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ABSTRACT 



This research concerns the development of cooperative control of two spacecraft 
mounted two-link manipulators as they reposition a common payload. Lagrangian formu- 
lation is used to determine the system equations of motion. Lyapunov stability theory is 
used to develop the cooperative control by using a reference trajectory and reference actu- 
ator torques. Polynomial curves represent potential reference trajectories. Numerical 
methods select specific reference trajectories to minimize the disturbance torque transmit- 
ted to the spacecraft during the payload repositioning maneuver. The reference actuator 
torques are selected to minimize weighted norms of the torques Analytical and experi- 
mental models of planar motion are used to study the performance of different cooperative 
controllers. The fifth order polynomial reference trajectory leads to superior performance 
in terms of spacecraft attitude accuracy, actuator torque magnitude, payload repositioning 
accuracy, and maneuver time. The higher order polynomial reference trajectory results in 
only minor improvement in performance. The experimental results verify the concept of 
cooperative control. 
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I. INTRODUCTION 



A. BACKGROUND AND LITERATURE SURVEY 

Robots are presently an integral part of industrial processes. They perform tasks with 
high precision, speed and reliability. These same features make robots attractive with 
regards to space applications. 

Space based robotics platforms experience conditions unlike those of their terrestrial 
counterparts. With respect to the dynamics of the systems, the most notable difference is 
the absence of a fixed base on which to locate the manipulators. The consequence of the 
difference is that the motion of the space based manipulator transmits forces and moments 
to its mounting base resulting in translation and rotation of the base itself. This of course 
impacts the location of the manipulator’s end effector. The problem is further complicated 
in that the disturbances are not simply a function of the present manipulator joint angles 
but are also a function of the joint angle histories preceding the current configuration. 
(Ref. 1) 

A number of approaches have been used for dealing with this coupling of joint angle 
histories and spacecraft main body attitude. Wang (Ref. 2) eliminates the problem by 
carefully defining what he expects of his dual-arm maneuverable space robot. He preposi- 
tions the manipulators such that they are configured to grasp the payload once the vehicle 
moves within range. After the manipulators are in position, their joints are locked while 
the spacecraft maneuvers to a location and attitude near the payload. Next, the vehicle 
approaches the payload in a straight line until the end effectors can grasp the payload 
While the manipulator joints remain locked, the vehicle repositions the entire rigid body 
system to the desired payload destination. At this point, the payload is released and the 
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vehicle backs away along a straight line. The repositioning of the payload is accom- 
plished by means of the vehicle attitude control rather than altering the joint angles in the 
manipulators. The manipulators themselves are not moved except when the attitude dis- 
turbance they impart to the vehicle is of no importance. Maintaining vehicle attitude dur- 
ing manipulator motion is not a requirement. 

Longman, Lindberg and Zedd (Ref. 3) calculate the disturbance torques caused by 
manipulator motion. This information is used to calculate reaction wheel commands 
which will compensate for the disturbance torques. In this way, spacecraft attitude control 
is maintained while the manipulator is repositioned. 

If the vehicle does not contain reaction wheels, the primary source of attitude control 
is probably reaction control thrusters Because fuel is consumable and hence mission lim- 
iting, firing thrusters to hold spacecraft attitude should be avoided whenever possible. 
Vafa and Dubowsky (Ref. 4) and Longman (Ref. 5) use similar approaches to eliminate 
the need for reaction thruster firings. Both techniques involve constructing a manipulator 
trajectory which involves revolving the manipulator in a small coning motion at interme- 
diate stages of the payload repositioning maneuver. This motion imparts a slow rotation 
of the spacecraft about the coning axis. Careful use of the coning locations permits repo- 
sitioning of the payload between any arbitrary locations (within manipulator reach) and 
attitudes while also changing the spacecraft to any desired attitude without the need for 
thruster firings. This technique does not, however, maintain a particular spacecraft atti- 
tude during the maneuver. 

Nakamura and Mukherjee (Ref. 6) use a technique called the bi-directional approach. 
This method represents a six degree of freedom (DOF) manipulator mounted on a space 
vehicle as a nine variable system (six joint angles and three spacecraft attitude angles) 
with six inputs (the manipulator joint torques). They attack the problem from both ends 
by integrating forward from the initial conditions and backwards from the desired final 
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conditions. A Lyapunov function guarantees that the two solutions will converge at some 
intermediate time during the maneuver. As in Ref 4 and Ref 5, the payload is reposi- 
tioned to the desired location and attitude and the attitude of the spacecraft main body is 
changed to its desired orientation. However, the main body attitude during the maneuver 
is not controlled. Furthermore, the joint angle trajectories calculated by this method con- 
tain rapid, large oscillations near the maneuver start and stop times and noncontinuous 
derivatives at the convergence time. 

Vafa (Ref. 7) succeeds in using a single space-based manipulator to control spacecraft 
attitude during a repositioning maneuver. He does this by employing a nine DOF manipu- 
lator. This manipulator has enough redundancy in its kinematics to control the end effec- 
tor location and attitude and the spacecraft attitude. Six DOF are allocated to 
repositioning the payload and the remaining three DOF are used to control the spacecraft 
attitude. 

Like Ref. 7, the primary objective of Chung, Desa and deSilva (Ref. 8) is to address 
the disturbances transmitted to the spacecraft by the manipulator motion. They also use a 
single manipulator with redundant kinematics. Because they use inverse kinematics to 
find the joint torques, the manipulator redundancy prevents the existence of a unique solu- 
tion. A solution is selected from among the infinity of possible solutions by means of 
minimizing a cost function of the magnitudes of the forces and torques transmitted to the 
base. 

Torres and Dubowsky (Ref. 9) also focus on the spacecraft attitude disturbances 
caused by manipulator motion. They recognize that for any given point in joint space, 
there is a direction of motion which produces minimum spacecraft attitude disturbance 
and a perpendicular direction of motion which produces maximum spacecraft attitude dis- 
turbance. A tool called the enhanced disturbance map (EDM) depicts these directions 
graphically. The EDM permits users to plan manipulator trajectories that lie on or near 
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zero disturbance paths. For non redundant manipulators, this may involve repositioning 
the spacecraft itself prior to the manipulator repositioning maneuver. If the manipulator 
has redundant kinematics, one can find a zero disturbance path to connect the manipulator 
trajectory endpoints without having to reposition the spacecraft initially. This technique 
considers only the location of the manipulator endpoint and not its attitude. 

Configurations with multiple manipulators have also been explored. The closed chain 
nature of these configurations prevent the use of some of the techniques already discussed. 
In addition, controlling multiple manipulators raises the issue of cooperation between the 
manipulators. 

Nguyen, Pooran and Premack (Ref. 10) develop a PD controller for a fixed base, two 
DOF, closed chain manipulator system. The system is linearized by means of Taylor 
series expansion about a point designated as the robot’s “home” point. Pole placement is 
then used to select controller gains 

Hu and Goldenberg (Ref. 11) derive an adaptive control scheme for multiple nonre- 
dundant manipulators mounted to a fixed base. In this reference, coordinated control 
involves controlling the motion of the grasped object, the contact forces between the 
object and its environment, and the internal forces within the object caused from being 
held by more than one manipulator. 

For a space based system, contact forces between the payload and its environment are 
less likely to be important. Walker, Kim and Dionise (Ref. 12) present just such an adap- 
tive controller. 

Coordinated control of multiple manipulators assumes a different meaning according 
to Yoshida, Kurazume and Umetani (Ref. 13). Although they propose a system with two 
manipulators, only one actually grasps the payload The other is used to provide counter- 
balancing torques to the spacecraft main body. The role of the second manipulator is sim- 
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ilar to a reaction wheel in that its primary function is to control spacecraft attitude rather 
than reposition a payload. 

Ahmad and Zribi (Ref. 14) apply a Lyapunov controller to a fixed base, multiple 
manipulator system. As in Ref. 12, they are concerned with controlling the payload posi- 
tion and its internal forces. To do so, the method requires sensors to measure the forces 
and moments created by each manipulator They also present an adaptive version to con- 
trol this system. 

While still addressing payload position and internal forces, Schneider and Cannon 
(Ref. 15) use a technique called object impedance control to achieve coordinated control 
among the manipulators. This method views the payload as being anchored to a desired 
location by a spring/damper system. 

B. DISSERTATION OVERVIEW AND OBJECTIVES 

This research is concerned with the cooperative control of a space based manipulator 
system with multiple manipulators handling a common payload. The scope is limited to 
planar motion in which the spacecraft is allowed to rotate but not to translate. These 
restrictions permit experimental verification in the Spacecraft Dynamics and Control Lab- 
oratory at the Naval Postgraduate School. The objectives of this research are to 1) develop 
a stable control law which facilitates cooperation among the manipulators as they reposi- 
tion the payload, 2) minimize joint actuator effort, 3) reduce the disturbance torque trans- 
mitted to the spacecraft main body by the manipulator motion, and 4) validate the 
analytical development with experimental results. 

Chapter II develops the analytical model in detail. Coordinate systems are defined and 
the equations of motion are derived. A technique for finding control torques which mini- 
mizes a weighted norm is presented. A globally stable control law is developed using 
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Lyapunov methods. The idea of using a reference trajectory to describe the motion is 
applied as are methods for choosing the reference trajectory. 

Chapter III verifies the analytical model with several test cases. The model is evalu- 
ated for compliance with the principles of conservation of kinetic energy and angular 
momentum. After establishing the validity of the model, results from simulations are pre- 
sented. The stability of the controller is illustrated as is the dramatic improvement in per- 
formance when a reference trajectory is included. Results from a simplified control law 
which is more practical to implement are included and compared to the complete control 
law version. 

Discussion of the experimental work is contained in Chapter IV. This chapter includes 
a description of the experimental setup. As might be expected, actual hardware demon- 
strated that there are differences between the ideal world of the analytical model and the 
real world of hardware implementation. 

The summary and concluding remarks are presented in Chapter V. This chapter also 
contains suggestions for future work in this field. 
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II. ANALYTICAL MODEL 



The analytical model represents a spacecraft with two manipulators attached. The 
manipulators have already firmly grasped an object hereafter referred to as the payload. 
The manipulators are about to reposition the payload with respect to the spacecraft. The 
ensuing dynamics between the spacecraft, manipulators, and payload are the topic of this 
research. What occurs before the manipulators grasp the payload and after they release it 
is beyond the scope of this investigation The scope is narrowed further by confining the 
motion to be two dimensional and allowing the spacecraft to rotate but not translate. 
These assumptions are consistent with hardware restrictions during experimental verifica- 
tion. 

A. COORDINATE SYSTEMS 

Figure 1 shows a schematic of the overall system. This diagram illustrates the rela- 
tionship between the coordinate frames used to develop the equations of motion. All 
angles are measured positive counterclockwise. The centerbody, manipulator links, and 
payload are assumed to be rigid bodies. Therefore, member lengths (Ab ^L2' 4tb ar, d 
4>), distances to member centers of mass (^ c q, ( , ^ 2 , / c r|, 4.r 2 , and ^ c p,), and the loca- 

tion of the left and right shoulders (^y, 9] 0 , ^ 0 , and Grq) remain constant. An inertial 
axis system is located on the centerbody at the point of rotation. A body fixed coordinate 
frame uses the same origin as the inertial frame but rotates with the spacecraft centerbody. 
The x-axis of this frame points to the centerbody center of mass. The centerbody attitude, 
6q, is the angle between the inertial x-axis and the spacecraft centerbody x-axis. Each 
manipulator link has its own set of body axes A coordinate frame attached to the left 



7 




Figure 1: Dual Two- Link Manipulator Configuration 
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shoulder aligns its x-axis along the longitudinal axis of manipulator Link LI . The attitude 
of this link, 0 L1 , is zero when the link lies on a ray extending from the inertial origin 
through the left shoulder. The attitude of Link L2 is defined by a coordinate frame 
attached to the left elbow. The attitude of this link, 0[ 2 , is zero when the link is parallel 
with the proceeding link. Link LI. Similar coordinate frames and definitions apply to the 
right manipulator. The payload attitude, 0p, is referenced to the inertial frame. The Carte- 
sian coordinates of the payload center of mass are also with respect to the inertial frame. 
A set of generalized coordinates which describe the system include the centerbody atti- 
tude, joint angles for all of the manipulator links, and payload attitude and position. 



Six joint actuators apply torques at the shoulder, elbow, and wrist of each manipulator. 



where the first element is the reaction wheel torque. The remaining elements are joint 
actuator torques. The first letter of their torque subscripts indicates left or right arm. The 
second subscript indicates shoulder (S), elbow (E) or wrist (W) 

B. EQUATIONS OF MOTION 

The equations of motion for this system are developed using Lagrange’s equations for 
a dynamic system with holonomic constraints. 




( 1 ) 



A reaction wheel applies a torque to the centerbody. The actuators can be grouped into a 
control vector 




( 2 ) 



d TaL^i _ aL 

dt[dqj dq 



= Q + A T a 



( 3 ) 



subject to constraints 
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Aq + A () = 0 



(4) 



where L = T - V 

T is kinetic energy 

V is potential energy 

q is the generalized coordinates vector 

q is the generalized velocities vector 

Q is the vector of applied nonconservative forces 
T 

A X are the constraint forces 

Beginning with Lagrange’s equation, the equations of motion can be rearranged into 
an alternate form. The inertia matrix, M, is a function of the generalized coordinates. 
Since the potential energy is a function of only the generalized coordinates, the partial of 
the Lagrangian with respect to the generalized velocities does not contain any potential 
energy terms. 



dL \ t ■ 

— = Mq 

dq 

Differentiating Eq. (5) with respect to time leads to 



(5) 



dt 



dC 



.TdM 



.TdM 



= Mq + q — = Mq + q — q 

dt ' _ dq - 



(6) 



Replacing the Lagrangian with expressions for kinetic energy and potential energy 
results in 



dL 

dq 



1 f .TdM . 
2 



q — q 

r’q 



dV 

dq 



Eq. (6) and Eq. (7) can be substituted back into Eq. (3) to produce 



Mq + 



1 f .TdM 



2 V- dq 



9 + 



dv 

dq 



- Q + A X 



(7) 



( 8 ) 
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The second term on the left-hand side of Eq. (8) contains the centripetal and Coriolis 



torques Replacing this with the G matrix leads to 

(7 V T 

Mq + G(q, q) + — = Q + A X (9) 

dq 

After substituting the matrix form of the generalized forces into the equations of 
motion, one has 



The following sections develop expressions for the inertia matrix, Coriolis and cen- 
tripetal accelerations, generalized forces, and constraints imposed by the closed chain 
geometry of the system. 

1. Inertia Matrix, M 

The inertia matrix is found by calculating the system kinetic energy and expressing 
it in the form 



The inertia matrix is the term bracketed by the generalized velocity vectors. The 
total system kinetic energy is the sum of the kinetic energy of all the pieces. 



(J\ T 

Mq + G+ — - Bu + A 1 X. 
f?q 



( 10 ) 




( 11 ) 




( 12 ) 



Kinetic energy of individual components can be found from 




(13) 



I; is the member moment of inertia about its center of mass 



co j is the angular velocity 
irij is the mass 

r is the inertial velocity of the center of mass 
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The centerbody angular rate and center of mass position vector are given by 



®o = 



r o = *.o*« 



(14) 

(15) 



Differentiating Eq. (15) results in the velocity of the centerbody center of mass 

r 0 = * C 0®0>'» ( 16 ) 

Substituting Eq. (14) and Eq. (16) back into the expression for kinetic energy (Eq. 
(13)) and collecting on the angular rate term leads to 



T o = 2 (I o + m (/rt |(i o 



(17) 



Similar developments are used for each of the remaining pieces in the system. For 
the left manipulator link between the shoulder and elbow (Link LI), the angular velocity is 
a combination of centerbody rotation and rotation of the link with respect to the center- 
body. 



“ L , = o 0 + o LI 



(18) 



The position vector to the center of mass is 



. r L1 =f LO cosG Lo S o + ^sinO LO > 0 + ^ cL| X L] (19) 

The First two terms in the position vector represent the location of the left shoulder. 
Differentiating the position vector gives the expression for the velocity vector. Because 
none of the coordinate axes used in the position vector expression are inertial, their rota- 
tion must be included as well. 



r L1 = VV OS0 Loyo— 






• LI 



(20) 



After Eqs. (18) and (20) are substituted into Eq. (13) and terms are grouped by 
angular rates, the kinetic energy is 

= 0o(O.5(I u +m L / c 2 L1+ m L1 ^ o ) (21) 
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+ ,,, Ll^cLl (sin0 LO sin(O l.O +O l l> +COsB L 0 C o S(e i 0 + 0, ,)) ) 

+ ().50 l1 (I L1 +ni L /; L1 ) 

+ Mli(>li +m L l^I.l +m Ll^n Z cll (S,n 0 1.0 Sm( 0 l,0 + ( Vl> +COS °LO COS(0 LO + 0 Ll))> 

The angular rate for the left forearm includes the centerbody angular rate as well as 
the angular rates of the body axes on Links LI and L2. 

W L2 = 0 O +0 L1 +0 L2 (22) 

This link’s center of mass position vector is 

r L2 = ^o cos0 LO X o+^o sil ' 0 i.„yr. + ^.i S L| Hl.2\ : ( 23 ) 

Differentiating the position vector gives the velocity vector. 

r L2 = Vv os0 Loyo-V o o sin() i o^ + *Li o, L,y LI H L 2 (o L2y LI ( 24 ) 

The kinetic energy expression for Link L2 is found after substituting Eqs. (22) and 
(24) into Eq. (13) and collecting terms with common angular rates. 

^L2 = < 0 - 5 (! L 2 + ni L2^cL2 + nl L2*Ll + ,1, L2^L0 ) (25) 

+ m L2^L0^1 ( sin0 LO sin (0 LO + °L1 > + COS0 LO COS ( °LC + 0 LI > > + m L 2*LI*cL2 COS0 L2 
+ m L2^L0^cL2 1 sin0 Lo Sm ^ 0 LO + 0 l.l + 0 L2^ + COS0 LO COS * 0 LO + 0 L] + 0 L2^1 ^ 

+ Oj | (0.5 (I L2 + (11 L2 ^ L| +i "l2^cL 2^ + ln L2^Ll^cL2 COS0 L2^ 

+ O.50L 2 n L2 + ni L2 f c 2 L2 ) 

+ Ml. <I L2 + m L2 Z LI + m L2 Z cL2 + 2n, L2^/cL2 COS °L2 

+ n, L2*L0*L I ( sin0 LO sin (0 LO + °L I > + COS °L0 COS < °L0 + 0 L I > > 

+ m L2WcL2 ( sin0 L O Sin (0 |.O + °|.| + °L2> + COS0 LO COS ( 0 LO + 0 L 1 + 0 L2> > > 

+ ML2 d L 2 + n, L2*cL2 + ">L2 VcL2 COS0 L2 

+ m L2*L</cL2 ( S ‘ n °L0 S,n ( °L0 + °l.l + °L2 ) + COS °L0 COS ( °L0 + °L 1 + 0 L2 > > > 
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+ 0 L |«L 2 n L2 + m L2 & 2 + m L2 « L / cL2 cosO L2 ) 

The development for the right manipulator kinetic energy parallels that for the left 
manipulator. For the upper arm portion (Link Rl), the angular rate is 

(0 R1 = d„ + o R1 (26) 

The position vector is constructed by finding the coordinates of the right shoulder 
and adding the vector from the shoulder to the center of mass. 

f Rl = *R0 COS ®R0*0 + *RO S ‘ n( *R0yO + *cRl* R ) (27) 

The time rate of change of the position vector is 

r R, =< Ro“o cos %yo-^o (0 o sin0 Ro s o + ^ C R,“R,.v R1 (28) 

After calculating the kinetic energy for Link Rl and grouping terms with common 
angular rates, the resulting expression is 



TrI - 0 0 ( 0.5 (Ir, +m R|* c R| 



n1 RI^Ro) 



(29) 



+ m R. VcRI ( sin0 RO Sin ,0 RO + °R | ' 1 + COS °R0 COS (0 RO • 



Or,))) 



+ O.50R,(I R1 +m R |/ c 2 R ,) 

+ 0 0 Or, (I r| + 111 K I R 1 +m RI^Rf/eRl ,Sin0 RO sin ^ (, R0 

Angular rate of the right forearm is 

®R 2 = 0(1 + Or, + 0 R2 



+ 0 R] ) +cos0 RO cos(O RO + 0 R ,))) 



(30) 



Its position vector is 



r R 2 - ^ 0 COS ®R 0 * 0 + *R 0 Sln ®R 0 y» + *RI* R , + ^cR 2 *r 2 (^) 

Differentiating Eq. (31) produces the velocity vector for Link R2 center of mass. 



r R2 = Z R0“0 COs0 R0y0-^0 W 0 sine R0^0 + ^R ) ®R 1 y R1 +^ c R2®R2y R1 ( 32 ) 
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The kinetic energy resulting from substituting Eqs. (30) and (32) into Eq. (13) and 
collecting common angular rate terms is 

.2 

Tri — 0 0 (0.5 ( Ir, + I"r2^cR 2 ,n R2^R 1 + n1 R2^R(d (33) 

+ ,,, R2WRI (si,,0 RO Sin(0 RO +e Rl) + COS °R 0 COS <°RO +0 R1 > > + m R 2*R /cR2 COS °R2 
+ m R2 WcR2 ( si "°R0 si " < °RO + °R I + °R2> + COs0 RO COS ( °R0 +0 R I + °R2> > > 

+ d R | (0.5 1 1 R2 + tn R 2^R I +m R2^cR2* + m R2^ R /cR2 COS0 R2* 

+ O.50 r i H R 2 "*■ ’ 1J R2^cR2^ 

+ ( 'o‘»RI (1 r; + ,n R2^RI +n, R2^cR2 + “ n, R2*R1*cR2 COS0 R2 

+ m R2^R0^R I ^ Sin0 RO S ' n * 0 RO + 0 Rp + COS0 R() COS * 0 RO + 0 r |) ) 

+ m R2VcR2 ,sin0 RO Sin(0 RO + ' , RI + °R2 ) + COS °R0 COS (0 RO + °R I + 0 R2>> > 

+ 0 O®R2 Or 2 + ln R2^cR2 + I "r2^ R /cR2 COS0 R2 

+ m R 2*RO*cR2 1 sin0 RO si " <% + °RI +0 R2 ) + COs0 RC COS ,0 RO +0 R1 + 0 R2> > > 

+ 0 Rl 0 R 2 n R2 + m R2^cR2 + m R2^Rl^cR2 COS0 R2^ 

Expressions for the payload are considerably simpler because inertial coordinates 
are available. The angular rate is 

«>,» = 0 i> (34) 

It is not necessary to describe the payload center of mass by passing through either 

shoulder as was the case with the manipulator links. The position vector is 

r p = X p fl x + Y p fl y (35) 

The velocity vector is also simpler because of the inertial frame. 

r p = X P N x + YpO, (36) 
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The payload kinetic energy is derived from substituting Eqs. (14) and (16) into Eq. 

(13). 

Tp = ' (ipOjUnyXp + Yp)) (37) 

After substituting the expressions for kinetic energy from Eqs. (17), (21), (25), 
(29), (33) and (37) into Eq. (12) and expressing the result in the matrix form of Eq. (3), the 
inertia matrix, M, is given by Eq. (38). Because the generalized coordinates for the pay- 
load are referenced to an inertial coordinate frame, the inertia matrix is decoupled between 
the payload and the rest of the system. Coupling does exist between the spacecraft center- 
body and each of the manipulators. 



M.. 


M ] 2 


M ,3 


M .4 


M 15 


0 


0 


0 


m 2 , 


M 22 


M 23 


0 


0 


0 


0 


0 


m 3 . 


M 32 


M 33 


0 


0 


0 


0 


0 


M 4, 


0 


0 


M 44 


M 45 


0 


0 


0 


m 5 . 


0 


0 


M 54 


m 55 


0 


0 


0 


0 


0 


0 


0 


0 




0 


0 


0 


0 


0 


0 


0 


0 


m,> 


0 


0 


0 


0 


0 


0 


0 


0 


ni,> 



Expressions for the individual elements in the inertia matrix are given by 



M 



55 




+ m 




2 

R2 



M 

M 

M 



45 

15 

44 






- M 55 + m R 2 ^Rl* C R2 COS0 R2 
= M 45 + m R2 ^Ro^ c R2 cos ( e Ri + 0 R2^ 

+ IrI + m R 2^Ri^ c R2 COs6 R2 + m Rl* C Rl 



+ m 



R2*R1 



(38) 



(39) 

(40) 

(41) 

(42) 
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( 43 ) 






M 14 = M 4I = M 44 +Z Ro( nl R| Zc RI +m R2^R|) COS0 RI 
+ m R2*R(/ C R2 COS ( 0 R 1 + 0 R2^ 



M 33 


= lj 0 + m 


L2* C L2 




(44) 


M 23 


" M 32 


= 


M 33 + m L2^I./ C L2 


cos9 1 2 


(45) 


M 13 


= M 3. 


= 


M 23 + m L2W C I,2 


cos ( 0 , , + 0 , 2 ) 


(46) 


m 22 


= M 23 


+ 1 


Ll + m lAl' C L 2 COS °L 2 + ra Ll* C Ll + m L 2 / Ll 


(47) 


M .2 


= M 2 . 


= 


M 22 + 2 LO< m U <C l. 


l +m L2 < LI> COS0 Ll 


(48) 



+ m L2W C L2 COS ( 0 Li +0 L2> 

M 11 = I 0 + M 22 + M 44 + m (/ C 0 + (lll l,l +m L2^ L0+ ( m Rl +m R2^R0 ( 49 ) 
+ 2 ^R0 ( m Rl^ C R1 + m R2^Ri^ COS0 R1 + 2m R2*R0* C R2 COS ^ 0 R1 + 0 R2^ 

+ 2/ L0 ( m Li^ C Ll +m L2*Ll) COS0 L 1 + 2m L2*L(/ C L2 COS ( 0 L 1 +0 L2^ 

2. Centripetal and Coriolis Matrix, G 

The G matrix contains all of the centripetal and Coriolis terms. It is most easily 
found using 



G(q, q) = 



CiV 2 ', 



q T C |8 >, 



(50) 



where the elements of are defined by the Christoffel symbol 



c ( j» - 1 

jk ? 



fdM: dM 



0 q, 






ik 






jk 



( \ 



c’q, 



(51) 
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The form of the G matrix for the system of Figure 1 is given as 



G = 




0 

0 

0 



°1 = -*L 0 ( ®L 1 + 2 Mli) ( m Ll * c L1 + m iAl )sil,n Ll 



_n1 L2*Ll* C L2®L2( 2 ®O + 2 0Ll + 0 L2) sin0 L 2 

~ m L2*LO* C L2 (20 O ((5 Ll +( ^L2) + < { »L1 + ° L 2> sin ^ G L ] + 0 L ;>> 

~^R0 ^ 0R 1 + 2 Mr«) ^ m R |^ C R l + m R2^Rl ^ S ' n0 RI 
- m R2<R/ c R 2 e R2 ( 2 0o + 29 R , +G R 2)sinO R 2 

-n, R2*Rc/ C R2^ 20 O* 0 Rl + 0 R2> + * 0 R1 + 0 R2> ) s ' n < 0 R] + 0 Ri) 

.2 .... 

G 2 = < LO 0 O (m L / C LI +nl L2^Ll ) sil,0 Ll " m L2*U* C L2°L2 < 20 O + 20 L1 + °L2) sin0 L 2 

+ m L2^LO^ C L2 0 O s ' n ( 0 L1 + 0 L2^ 

G 3 = m lAl* c L2 (0 Ll + e L 2> 2si " 0 L2 + "hA(AT2 0 o sin (0 L , +0 L2 ) 

G 4 = ^RO 0 O ^ n, Rl^ C RI '"r 2^R1^ sin0 Rl _ ni R2^Rl^ C R2 0R2 ^ 20 O + 20 R1 + 0 R2^ s ' n0 R2 
+ m R2W C R2 0 O sin < 0 Rl + °R2> 

G 5 = ,T1 R2^R 1^ C R2 ^ 0R1 + 0 R2^ S ' n0 R2 + m R2^RO^ C R2 0 O S ' n ^ 0 R 1 + 0 R2^ 



( 52 ) 



(53) 



(54) 



(55) 

(56) 



(57) 
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3. Generalized Forces, Q 

The generalized forces are found using the principle of virtual work. When there 
is no reaction wheel on the centerbody, the system does not experience any external forces 
capable of performing work. Six joint actuators apply torques at the shoulder, elbow, and 
wrist of each manipulator. 



u 6 = 



iT 



U LS U LE U LW U RS U RE U RW 



(58) 



U(-) is simply the joint actuator subset of the complete actuator torque vector, u. The 
total virtual work is the sum of the torques applied to the individual bodies times their vir- 
tual angular displacements. 



N N 

5W = £ 5W ; = £ (MjjSGj (59) 

i = 1 i = 1 

When the left shoulder joint actuator applies a positive torque on Link LI, a nega- 
tive torque is also applied to the centerbody. The virtual work performed by the left shoul- 
der motor is 



8W L s = u LS (5e„ + 60,,— 86,,) (60) 

where the positive angles are those associated with the change in Link LI attitude and the 
negative angle is associated with the change in centerbody attitude. The left elbow actua- 
tor makes a positive contribution to Link L2 attitude and a negative contribution to Link 
LI attitude. 

6W LE = u le (S6 0 + ^®L1 + _ U LE^ 0 L2 

The joint actuator at the left wrist makes a positive change in the payload attitude 
and a negative change in Link L2. 

6W LW = U LW (60 P " 60 O ~ 60 L 1 ~ 60 L2 ) (62) 
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The right shoulder actuator makes a positive contribution to Link R 1 attitude and a 
negative contribution to centerbody attitude. 

^RS = U RS^®() + ^®R1 = U RS^®R] 

Link R2 has a positive virtual displacement due to a positive torque at the right 
elbow. The same torque causes a negative virtual displacement of Link R1 . 

^ W RE = U RE^®0 + ^R1 + ^®R2 ~ " ^®R|) = U RE^^R2 

The right wrist actuator has a positive influence on the payload and a negative 
influence on Link R2. 

^^RW = U RW — ~ ^®R 1 _ ^®R2^ (^) 

Gathering Eqs. (60)-(65) together produces 

5W = ( -u L w _u Rw) 50 O + ( U LS~ U LW^ 50 L1 + ( U LE -U Lw) S ®L2 

+ < U RS - U RW> 6e Rl + < U Rli - U RW> 69 R2 + < U LW ~ U RW> 59 P 
With respect to the system equations of motion, the generalized force correspond- 
ing to a particular generalized coordinate is that portion of the virtual work associated with 
the same generalized coordinate. Now Eq (66) can be transformed into a matrix form. 

% = B< *-6 

where B is the control influence matrix given by 

0 0 -10 0-1 
10-100 0 
0 1-10 0 0 
B = 0 0 0 1 0 -1 
6 0 0 0 0 1 -1 

001001 
0 0 0 0 0 0 
0 0 0 0 0 0 



(67) 
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The only efTect of a positive reaction wheel torque applied to the spacecraft is to 
alter the centerbody attitude in a positive direction This manifests itself in the control 
influence matrix in the form of another column. This new column is all zeros except for a 
single one corresponding to the location of the reaction wheel torque in the u vector. With 
the reaction wheel torque as the first element in the control vector as in Eq. (2), the com- 
plete control influence matrix is 



10 0 -10 0 -1 
0 10-100 0 

0 0 1 -1 0 0 0 

0 0 0 0 1 0 -1 
0 0 0 0 0 1-1 
000 1 00 1 

0000000 
0 0 0 0 0 0 0 



(69) 



4. Constraints Matrix, A 

Because the eighth order system under consideration has only four degrees of free- 
dom, an additional four equations are needed to describe the constraints. The eight gener- 
alized coordinates are not independent. The constraint equations embody the information 
that the manipulators are both grasping the payload forming a closed chain system. The 
constraints matrix is derived by writing the system constraints in the Pfaffian form as 

Aq + A 0 = 0 (70) 

These equations come from geometric relationships of expressing the payload cen- 
ter of mass Cartesian coordinates in terms of the other generalized coordinates. 



*LO COS(0 O + e LO ) + *LI COS(0 O + 0 LO + 0 LI> + *L2 COS ,0 n + °L0 + °L1 + °L2 ) + ^p c ° s0 p = x p 
*LO sin < 0 O + ( W +<L,sin( 0 O + 0 LO + 0 L I > + ^L2 sin(O « 1 +0 LO + 0 Ll +,) L2 ) + V in0 P = >'p 

*R0 COS ^ 0 O + 0 RO^ + ^R1 C0S <°0 + °R0 + 0 Rl ) + *R2 C0S ^ 0 O + 0 RO + 0 RI +0 R2* ~ ^P _ cos0 p = X p 
W in(0 O + 0 RO> + *Rl sin '°O + 0 R< J + 0 RI ) +^2 sin(G O +0 RO +0 Rl +0 R2> " U P ~ V sin0 P = >P 



(71) 
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To get the Pfaffian form of Eq. (70), differentiate Eq (71) and rearrange terms. 



The result is 



A n 


A .2 


A u 


0 


0 


A 16 


-1 


0 


A 2. 


A 22 


A 23 


0 


0 


A 26 


0 


-1 


A 3, 


0 


0 


A 34 


A 33 


A 3, 


-1 


0 


A 41 


0 


0 


A 44 


A 45 


A 46 


0 


-1 



Oo 

<>L. 

0L2 

hRi 

<»r: 

G P 

X P 

Yr 



0 

0 

0 

0 



(72) 



The constant term, Aq, is a zero vector. The individual element in the constraints matrix 
are given by the following equations 



A i6 = -^CpSinGp 


(73) 


A 2(3 — ^CpCOS0p 


(74) 


— (^j> — ^Cp) sinGp 


(75) 


A 46 = _ C^p— ^c p ) cosGp 


(76) 


A 45 = ^R2 COS ^®0 + ®R0 + ®Rl + ®R2^ 


(77) 


A 44 = A 45 + ^RI COS ^®0 + ®R0 + ®RI^ 


(78) 


A 41 = A 44 + ^ R() COS ^®0 + ®R0^ 


(79) 


A 35 = ~*R2 sin (®o + ®R0 + ®R1 + ®R2^ 


(80) 


A 34 ~ A 35 _ ^Rl Sin ^ 0 () + 0 RO + e Rl^ 


(81) 


A 31 = A 34~^R0 S ‘ n ^®0 + ®R0^ 


(82) 


A 23 = ^L2 COS ^®0 + ®L0 + ®Ll + ®L2^ 


(83) 
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^22 ^23 + I ( 0 O 0 LO ^ 0 1 1 ^ 


(84) 


^21 = ^22 + \ „ C0S + ®L()) 


(85) 


^13 ~^L2 S ‘ n (®0 + ®L0 + ®Ll + ®L2^ 


(86) 


^12 = ^13 “^1.1 S ' n (®0 + ®U) + ®lj) 


(87) 


A I1 = A l2 - *L0 S ‘ n ^0 + ®L()) 


(88) 



If the manipulators are mounted on a fixed platform rather than a rotating base, an 
additional constraint equation is included in the A matrix. The constraint is that Gy is con- 
stant and therefore 

o 0 = o (89) 

This constraint is augmented into the A matrix by adding a fifth row. The first ele- 
ment in the row is a one. The remaining seven elements are all zeros. 

C. SIMPLIFIED EQUATIONS OF MOTION 

The potential energy term is zero because motion is confined to the horizontal plane 
and the system is composed of rigid members The inertia matrix, G matrix, B matrix, and 
constraints matrix can be found from the results of the previous sections. The remaining 
unknowns are the actuator torques and the Lagrange multipliers By using the equations 
of motion and the Pfaffian form of the constraints, one can eliminate the Lagrange multi- 
pliers. The time derivative of Eq. (70) is 

Aq + Aq = 0 (90) 

Solving Eq. (10) for q and substituting the result into Eq. (90) permits one to find an 
expression for the Lagrange multipliers. 
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X = (AM 'a T > ’(AM '(G-Bu)-Aq) (91) 

The inertia matrix is always a square matrix with full rank and therefore invertible. To 

-]T. 

investigate the invertiblity of AM A , begin by creating a 4x4 matrix out of the third, 
fifth, seventh, and eighth columns of the constraints matrix. 



A ,3 


0 


-1 


0 


A 23 


0 


0 


-1 


0 


A 35 


-1 


0 


0 


A 45 


0 


-1 



(92) 



Inspection of this submatrix reveals that all of the rows and columns are linearly inde- 
pendent even if A ]3 = A 23 and A 35 = A 45 . Therefore, the A matrix always has rank of 4. 

-1 T 

The 4x4 matrix product AM A will also always have rank of 4 and is therefore invert- 
ible. Eq. (91) can be substituted back into the equations of motion (Eq. (10)) leaving the 
actuator torques as the only unknowns. The resulting equations of motion in which the 
Lagrange multipliers have been removed and potential energy is zero are 



Mq + G = Bu 



(93) 



where 



G 



= G-A T (AM *A T ) ’(AM *G- Aq) 
B = (i-A T (AM _ 1 A 1 ) 'aM _1 )b 



(94) 

(95) 



D. REFERENCE TORQUES 

Given a reference trajectory of the payload with known displacements, velocities and 
accelerations, one can use the simplified equations of motion (Eq. (93)) to solve for the 
actuator torques that will produce the reference trajectory. 
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( 96 ) 



M rei3 rc r + G icr = B re( u ref 
The equations for specific elements in the matrices of Eq. (96) are the same as already 
presented. The subscript “ref’ merely means that the displacement, velocity and accelera- 
tion terms are the values from the reference trajectory. 

In this study, the total number of actuators is more than the system degrees of freedom. 
This situation is caused by the geometric constraints of multiple manipulators handling a 
common object. As a result, there are an infinity of solutions for the reference torques. 
One method to select a soecific solution is to establish a cost function. An obvious cost 
function is to minimize a weighted norm of the actuator torques. 



J = 



1 

2 




W u 

ii 



ref 



(97) 



The problem now becomes one of minimizing the cost function (Eq. (97)) subject to 
the constraint that the reference equations of motion (Eq (96)) are satisfied Augmenting 
the cost function with the constraint by means of another Lagrange multiplier leads to 



J - 2 “JXv + ? T (Ac-c-^rV, - G ,cf) (»*> 

The minimum of the augmented cost function is found by taking the gradient of Eq. 
(98) with respect to the reference torques and with respect to the Lagrange multiplier. 
Each of the gradients is set to zero. 

V u > = 0 = w u +B r ' cf r (99) 

-rel 1 ^ 1 

V 7 J = 0 = B rcf u rer -M rcr 9 ref -G ref (100) 

Eqs. (99) and (100) are two equations in two unknowns (y, u ). Eliminating y 
results in an expression for the reference actuator torques 
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( 101 ) 



-Vr = w >rcr(Br«rW;'B r ' cf ) (M rcf q rcf+ O ref ) 

„ ~ T 

Although the matrix product B [ef w~'B rff is an 8x8 matrix, it is not invertible. A 
pseudo-inverse is needed because the system has only four degrees of freedom. There- 
fore, the matrix product B ref w~'B ref is rank deficient and has a rank of four at most. This 
expression for reference actuator torques minimizes the augmented cost function (Eq. 
(98)) at each instant in time. Although the value for the reaction wheel torque is calcu- 
lated, it is not minimized by this function. The reaction wheel torque profile is dictated by 
the disturbance torques transmitted to the centerbody as a result of manipulator and pay- 
load motion. For a given reference trajectory, an infinite variety of joint actuator torques 
can produce that trajectory. However, a given reference trajectory has only one reaction 
wheel torque profile that is common to all the infinity of joint torque combinations associ- 
ated with that trajectory. Equation (101) selects from among the infinity of joint actuator 
torques the one combination that minimizes the weighted norm cost function. Although 
the selection is limited to a single choice. Equation (101) also produces the correct reac- 
tion wheel torque for the given reference trajectory. 

E. LYAPUNOV CONTROLLER 

This material in this section is based on Ref. 16. The purpose of any control law is to 
provide system performance that satisfies a specification. As a bare minimum, the control 
law must keep the system stable. Because of the highly nonlinear nature of this spacecraft 
robotics system, most control laws simply do not apply The motivation behind using 
Lyapunov methods is to develop a control law with guaranteed stability. Recall the equa- 
tions of motion of the manipulator system are 

Mq + G = Bu (102) 
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Solving Eq. (102) for q results in 



q - M 1 (Bu-G) 



(103) 



Substituting Eqs. (94) and (95) back into Eq (103) and grouping terms according to 
the form 



q = CjU + C 9 q + C, 



(104) 



leads to the following expressions 



,-l 



T , A xl -\ .T, 



-l 



,-l 



C. = M {1-A (AM A ) AM }B 



-1 » T / A W -1 A T 



-1 



C,, = -M A' (AM A ) A 



.-i .J/mi-i * t 



-l 



,-i 



C 3 = M {A (AM A ) AM — I J G 
Similarly, the reference maneuver accelerations can be expressed as 



(105) 

(106) 
(107) 



q - C, u r +C^ q r +C, (108) 

-ref 1 re r ref 2 re i ref -’ ref 

where again the reference subscripts on the C matrices indicate that reference maneuver 
values need to be used in their calculation. Let error quantities between the actual vari- 
ables and their reference maneuver counterparts be defined by 



5q = q-q 



ref 



Sq = q - q rcf 



8 9 = 9 - 9 rc f 

Now define an error Lyapunov function as 

U = 0.5 (5q • 5q) + f (5q) 
where f(5q) > 0. Differentiating Eq. (112) results in 



(109) 

( 110 ) 
( 111 ) 

( 112 ) 
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(113) 



0 = 5q 



•5q + V “ f 5q- 

i 1 



Let 



F = 



gf gf 
5(5qj) 5 (5q 2 ) 



-T 



af 

a(5q ? ) 



Then Eq. (114) can be rewritten as 



(114) 



U = 5q • (5q + F) (115) 

Substituting Eq. (104) and Eq. (108) into Eq (111) and then Eq. (1 1 1) into Eq. (115) 
produces 



0 = 5q- {(C 1 u-C lre U rc P + (C 2 q-C 2 ^q rcf ) + (C 3 -C 3 J+F} (116) 

If one lets the quantity inside the brackets of Eq (116) equal -K v 5q where K y is a 
positive definite matrix, then one is guaranteed that U < 0 and therefore the system will be 
stable in the Lyapunov sense. Solving Eq. (116) for command torques, u, leads to 



u = C t {-K v 5g + C, t u ref - <C 2 q-C 2 ji rcf ) - (C 3 -C } J -F) (117) 

Ci is an 8x7 matrix so Cjt is its pseudo inverse Equation (119) finds the torques that 
should be used rather than the reference torques. All that remains is to choose a function 
for f (5q) . One can choose 

f(5q) = “8q 1 K p 5q (118) 

where like K v , is required to be positive definite. Selection of values for the gain 
matrices is beyond the scope of this work. The simulations included in the next chapter 
use diagonal matrices with uniform values simply as a matter of convenience. One might 
try to adapt the linear quadratic regulator (LQR) problem to find more optimal gains. 
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After substituting Eq. (118) into Eq. (114) and that result into E:q. (119), one obtains the 
final form of the Lyapunov controller. 



= C,t j-K v 6g + C, 



u - (C. 

rcr ref 



i- C 2,JrJ " (C 3 -C 3 J -KpSgJ (119) 



If the differences between the reference trajectory and the system dynamics are small, 
the Lyapunov controller approaches the form of a proportional plus derivative (PD) con- 
trol law. 



F. REFERENCE TRAJECTORIES 

The reference trajectories describe the nominal path that the system follows in moving 
from the initial conditions to the desired final conditions. One need only specify reference 
trajectories for as many generalized coordinates as there are degrees of freedom. In effect, 
the generalized coordinates can be divided into two sets. One set contains the minimum 
number of coordinates needed to completely describe the system. The second set contains 
all remaining coordinates, (redundant coordinates). The choice of which generalized 
coordinates to specify is entirely arbitrary A reasonable choice includes the payload coor- 
dinates and centerbody attitude since the user will probably be especially interested in 
these generalized coordinates. The redundant coordinates are the four manipulator joint 
angles. Given reference trajectories for the minimum number of coordinates exist, the 
redundant generalized coordinates can be derived. This research assumes trajectories are 
available which define displacement, velocity and acceleration for the centerbody attitude, 
payload attitude, and payload center of mass coordinates (Xp, Yp, 0|> and 0 O ). 

1. Calculating Redundant Coordinates 

Figure 2 illustrates the relevant geometrical relationships to find the joint angles of 
the left manipulator. Xp, Yp, 0p and 0 q are obtained from the reference trajectory Point 

LS is the left shoulder joint. It has Cartesian coordinates given by 
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Figure 2: Deriving Left Manipulator Joint Angles 



LS x = < lo cos(,, o + 0 l.o) ( 120 ) 

LS y = *L0 si " ( V°L0> ( 121 ) 

Point Q is the joint between the manipulator end and the payload. The Cartesian 
coordinates of this point are 



Q x = x p-< c P C0se P O 22 ) 

Q y = Y p-*cP sin0 p (123) 
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The distance between the left shoulder and Point Q is given by 

LSQ = S x ) 2 + (Q y -I.S v ) 2 



The inertial angle formed by the vector from LS to Q is 

P } = atan 



^q‘”ls 



(124) 



(125) 



The dimensions of the triangle formed by the manipulator joints are known. Using 
the law of cosines, the interior angles at the shoulder and elbow can be found from 



= acos 



p. = acos 



tf 1 + LSQ 2 -*M 
21. .I.SQ J 






2 \ 



(126) 



(127) 



All that remains is to algebraically construct the manipulator joint angles from 
other angles as follows 

g li = P, + P : -‘°„ + « L „, ( 128 ) 

e L2 = p, + i8o° (129) 

The development for the right manipulator is similar. Its geometry is depicted in 
Figure 3. 

Point RS is the right shoulder joint with Cartesian coordinate 

(130) 

(131) 



RS x = *RO COS( °o + 0 KO> 



RS y = ^0 Sin(( ’.. +0 RO> 

Point P is the joint between the manipulator end and the payload. The Cartesian 
coordinates of this point are 

P x = Xp+ (£p - £ cP ) cosO p (132) 

Py- Y p +(/ p -< cP )*inO p (133) 

The distance between the right shoulder and Point P is given by 
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Figure 3: Deriving Right Manipulator Joint Angles 



RSP = ./(P -RS ) 2 + (P - RS ) 2 

"v x x y y 

The inertial angle formed by the vector from RS to P is 



P = atan 



P -RS 

y y 

P - RS 



(134) 



(135) 



From the law of cosines, the interior angles at the shoulder and elbow are 



p, = acos 



'*ri + RSP ^r: 

2Z r1 RSP 



(136) 
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P 6 = acos 



(137) 




The geometry in Figure 3 gives the manipulator joint angles based on the other 

angles. 



Recall from the discussion of the Lyapunov controller that torques are calculated 
based not only on the generalized coordinates but their velocities and accelerations as 
well. The redundant coordinates have just been found, but the redundant coordinates 
velocities and accelerations must still be developed. 

Differentiating Eqs. (122) and (123) expresses the velocity of Point Q. 



But the coordinates of Point Q can also be expressed in terms of left manipulator 
variables. 




(138) 




(139) 



Qx = Xp + V c p sm,, p 



(140) 



Qy = Yp-V cp COs0 p 



(141) 




Qy = *LO si " (0 O + °Lo) + *Ll sin(0 O +< ’l.O +O |.l ) + l \.2 S "' < °0 + °1.0 + °L1 + 0 L2 ) ( 143 ) 

Differentiate these equations and rearrange the terms in the form of 




(144) 



where 




(145) 




(146) 




(147) 
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i m i { : o.s(ii 0 1 o (il . 0, ( , o, ,) 



(148) 
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' V I. •’> l \ ;‘ os <"o ' °I 0 "’ll ' "i "V 1 ", o * °n> • *1 ««* < V V ( 150 ) 

1 cl) manipulator joint velocities arc found by rearranging l : quation (144) 

(151) 

where I i]s ( 140) and (141) provide the expressions for g\ and g\ respectively. 

Using the same approach to find the joint velocities of the light manipulator. Point 
I’ is expiessed as a function of right manipulator variables 

(152) 

(153) 

DilVcientiate these equations and re.urange the teims in the form of 
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« n 4 



(V 



K) 



(154) 
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Right manipulator joint v elocities are found b\ rearranging l quation (154) 
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where expressions for P x and p y are found by differentiating Eqs (132) and (133). 

P x = sinOp 



(162) 



P y = Yp + 0p(^ p - ^ cP ) cosOp (163) 

Manipulator joint accelerations are found by differentiating the expressions for 
velocity (Eqs (144) and (154)). 



(164) 
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(165) 



Solving for joint acceleration gives 
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(166) 



(167) 



where the accelerations of Points Q and P come from differentiating Eqs. ( 1 40)-( 141) and 
Eqs. (162)-(163). Derivatives of the D matrices are constructed by differentiating Eqs. 
( 1 45)-( 1 50) and Eqs. (155)-(160). 

2. Selecting Reference Trajectories 

Any path which connects the associated endpoints can be a reference trajectory'. 
To help ensure that the spacecraft and payload do not experience any unnecessary jerk or 
excitation of flexible structures, one might further constrain the path such that the veloci- 
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ties and accelerations are zero at the endpoints. Because a reaction wheel is required to 
maintain spacecraft attitude, the reaction wheel torque history is a prime candidate for 
optimization. Possible performance indices include the integral of the absolute value of 
reaction wheel torque 

V 

1 = J|u„ hccl |d. (168) 

l o 

or the maximum reaction wheel torque. 

J = max (|u„ hce| j) (169) 

A rigorous method for reference trajectory selection is to develop an optimal con- 
trol solution to the two point boundary value problem. The performance index in the opti- 
mal control problem is given by 

= Jk [x (I) , U (t) , Ij^ 1 (170) 

Using Eq. (168) as an example, 

L = |uj = |Du! (171) 

where 

C U = [ U wh U LS U LE U LW l, RS U RE U R\v]^ ( 172 ) 

and 

D = [i o o o o « q] (173) 

The state equations must be formulated as first order differential equations as 

* = r[x (t), u((),t] (174) 

Because the system dynamics of my problem are second order differential equa- 
tions, the state vector for the trajectory optimization is a combination of generalized dis- 
placements and velocities. 
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X 



(175) 



fx -tT 
L<i ‘i 



The resulting state equations are 
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M 'b 




-M 'g 



(176) 

where G and B are the same matrices as already found in Eqs. (94) and (95) respectively. 

Desirable boundary conditions are such that the payload is at rest with zero accel- 
eration at the beginning and end of the repositioning maneuver. However because the 
state vector does not contain accelerations, they cannot be specified as a boundary condi- 
tions. If the state vector is increased to include accelerations, then the first order state 
equations involve third order derivatives of the equations of motion rather than second 
order equations. This prevents including payload accelerations as part of the boundary 
conditions. To permit further development of the optimal control problem, the boundary 
conditions will be limited to desired positions and zero velocity. 



x(, 0> = [q<v 

X(t f ) = [q(1 f ) O.J 



(177) 



(178) 



The Hamiltonian formed by combining the performance index with the state equa- 



tions is 



H[ x(t) ,u(t),M.),3 = L [x(u, u(t),t] +xT (Of[x(t), u(t),l] (1*79) 

The performance index and the state equations are both linear with respect to the 
control vector, u. The consequences of this are that one cannot find a minimum by taking 
the gradient with respect to u and setting it equal to zero. The applicable control form is 
bang-bang. Separating the Hamiltonian into those terms which premultiply u and those 
which do not leads to the control law 
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U * = U m a X S, » n<II l> 



( 180 ) 



where 



H = H () + ll,u 

The other equations which must be satisfied are 

x T - - an - - dL 

dx dx dx 



( 181 ) 



( 182 ) 



Because the performance index is only a function of u, the first portion of the 
above necessary condition is trivial. 



dh 

dx 



= 0 



1x16 



( 183 ) 



— is not as easily found. The M, G, a, and A matrices are all functions of the state 

dx 

vector. In addition, the complexity is increased by several matrix inversions in the expres- 
sion of f in the 6 and B matrices. Although an analytical expression may be theoretically 
possible, finding it was found to be extraordinarily tedious. 

Recall, however, that the usefulness of the reference trajectories is to specify the 
generalized coordinates, velocities, and accelerations. Therefore, a convenient form for 
the reference trajectory is as a polynomial function of time. The following development 
uses the payload attitude generalized coordinate to illustrate how the polynomial reference 
trajectories are applied. Let 

A0 P = e p (t f )-e p (t 0 ) (i 84 ) 

where to is the maneuver start time and tj- is the final time. The duration of the maneuver 
is the difference between tf and to. 0p(to) and 0p(tf) are the initial payload attitude and the 
desired final attitude respectively. If the desired reference path for the payload attitude in 
moving from initial to final conditions is a curve which can be represented as a polyno- 
mial function, f(t), where t is simply normalized time 
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I = 



o-g 

Or-U 



(185) 



then 



0 p (I) = O p (l (J ) +f(t) (AOp) 



d P to = r (x) ( ao j 



Op (i) = r (x) ( ao p ) 



'r-'o 



(186) 

(187) 

(188) 



V<‘ ( - ‘o> V 

In order for Eq. (186) to produce the correct initial and final values for o p , the 

* r«f 

polynomial must be such that 

f(x=0) = 0 (189) 

f(x= 1 ) = I (190) 

To produce zero velocity and acceleration at the initial and final conditions 
requires that f(x) also satisfy 

f (x=0) = 0 (191) 

f (x=l) = 0 (192) 

f'(x=0) = 0 (193) 

f"(x=l) = 0 (194) 

The minimum order polynomial which satisfies the boundary conditions of Eqs. 
(1 89)-(l 94) is 



f (x) = 6x 5 - 1 5 x 4 + 10x 3 



(195) 



The expressions for payload reference trajectory using the fifth order polynomial 



become 



0 P (0 = 6 p (t 0 ) + (6x 5 - 15x 4 + 10x 3 ) (A0 p ) 



ref 



(196) 
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(197) 



) (t) = (30t 4 -60t 3 + 30t 2 ) (AG P ) 

r ref * 



vr V 



G,> (t) = (120x - 180r + 60x) (A0 P ) 

r ref * 



(198) 



ut f -t 0 r; 

The polynomial reference trajectory is also be applied to the other generalized 
coordinates which form the minimum set to describe the system (i.e. centerbody attitude 
and payload center of mass coordinates). The redundant generalized coordinates are cal- 
culated from the reference coordinates as described earlier. 

Higher order polynomials can increase the complexity of the path but offer the 
advantage that an infinity of polynomial coefficients satisfy the position, velocity’, and 
acceleration boundary conditions. The selection of the coefficients affords an opportunity 
to optimize the reaction wheel torque. In this system, manipulator actuator torques are 
internal while the reaction wheel torque is the only external torque. Therefore, the reac- 
tion wheel torque will be equal to the rate of change of angular momentum which can be 
calculated directly from a reference trajectory. This technique is more computationally 



efficient because it does not require the construction of the o and B matrices. 

In general, the angular momentum about the inertial origin for each member of the 
system is 



II = I (o + m ( r xv) (199) 

-i ‘~i i i i v f 

where i f is the moment of inertia of the i lh body about its center of mass 
m is the angular rate of the i 11 * body 
in is the mass of the i lh body 

r. is the inertial position of the i body center of mass 
v is the inertial velocity of the i 1 * 1 body center of mass 
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The angular rate, position and velocity vectors were previously developed in con- 
nection with determining kinetic energy. Those expressions require some coordinate 
transformations to express all the terms with respect to the inertial coordinate frame. The 
change in angular momentum is found by differentiating Eq (199) to produce 

it = I <o + m (r X a ) (200) 

— 1 '—i 1 i i 

The total system change in angular momentum is the sum of change in angular 
momentum for each of the members. After collecting terms with common angular veloc- 
ity or acceleration terms, the expression for the system change in angular momentum is 
given by 
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( 201 ) 



I! = I p Op + m p <X p Yp-XpYp) 



+ ^L1 [ J L1 +I L2 + ,11 Ll l cLl +m L2 , cL2 + m L2*Ll + ni Ll*L0*cLl COS0 L1 + ,11 L2 I L0 1 L1 0050 



LI 



+ 2 in. J. .1 . -cosO. + m. -I. J , .cos ( 0 . . +0 ) 1 

L2 Ll cL2 L2 L2 LO cL2 LI 12 J 



+ 61 7 r I. - + in. -I 2 . - + m. J, .1 , ^cosO. - + m, J. J , ^cos (0. . + 0 ) 

LZ \_ L2 L2 cL2 L2 Ll cL2 L2 L2 LO cL2 Ll t2 - 



+ <, R1 [}r\ +I R2 + ,11 R1 1 cR 1 +ni R2*cR2 + ni R2*Rl + ,11 R 1 ’ro'cRI COS °R 1 + ni R2*R0*Rl COS ^. 



R 1 



+ 2m R2 , Rl , cR2 COS °R2 + m R2 , R0 , cR2 COS ^R1 + 0 r 2 * D 

+ [} r 2 + m R2 , cR2 +ni R2 1 Rl l cR2 COS °R2 + ,11 R2 l R0 1 cR2 COS ( °R1 + ° R2 , _ 

-»-^Ll^L2 [- 2 m L 2l L) l cL 2 sin0 L 2 - 2 m L 2 l L 0 l cL2 sin ( 0 LJ +» L2 )] 

+ 0 L1 [“ ni Ll*L0^cLl S " 10 L1 ” ni L2*L0*Ll S ‘ ,10 L1 ” ni L2^LO*cL2 S * n ^L1 + 0 L2^ 1 
+ (i L2 I” ni L2 , Ll , cL2 sin0 L2 " m L2 , LO , cL2 Sin <°L1 + 0 L2> I 
+ 0 R1 0 R2 1“ 2m R2*Rl , cR2 S ' n0 R2 “ ^ ,11 R2*R0 1 cR2 S ‘ !1 f0 Rl +0 R2^ 1 
+ 0R1 [- m R] , R0 , cRl s,n0 R] “ m R2 l RO , Rl S ‘ n0 Rl ” m R2 , R0 , cR2 S ‘ n f0 Rl + 0 R2^ 1 
+ 0 R2 [” m R2 , Rl 1 cR2 S " 10 R2” ni R2 1 RO*cR2 S * n ^ 0 R1 +0 R2^ 1 

Any polynomial reference trajectory that satisfies the initial condition concerning 
displacement cannot have a constant term. Polynomials which satisfy the velocity and 

acceleration initial conditions must not contain linear or quadratic terms. The general n 111 
order polynomial reference trajectory' has the form 

f(x) = + Vf 11 ’ 1 + Vl 1 * 1 " 2 + ••• + a 5 x 5 + a 4 x 4 + a 3 x 3 (202) 

Derivatives are 

f (t) = na n x n_1 + (n-Da^jT 11 ' 2 + (n-2)a, 1 . 2 T T1 ' 3 + ... + 5a 5 x 4 + 4a 4 x 3 + 3a 3 x 2 (203) 
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f"(i) = n(n-l)a,,x n ' 2 + (n-l)(n- 2 )a n .ii n ’ 3 + (n- 2 )(»- 3 )a n . 2 i n ' 4 + + 2 ()ajt 5 + 12 a 4 i 4 + 6831 (2U4) 

When x=l and the final conditions, f(l) = 1, f’(l) = 0 nad f '(1) = 0, are substituted 
into Eqs (202)-(204), these equations can be put into matrix form 
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... 1 11 
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11 ( 11 - 1 ) 


(n- 2 ) 
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0 




Jl(!l- 1) (11-1) (11-2 


) (n- 2 ) (n-3) 


... 20 12 6 



(205) 



The column vector of polynomial coefficients can be partitioned One segment, 
8543 , contains the coefficients for the third, fourth, and fifth order terms in Eq. (202). The 



other segment, a^gh, contains all of the coefficients of order six and higher. 



a = h ’ eh (206) 

_ 8 543 _ 

The W matrix can be partitioned accordingly. 

w = [ w h, e h w J ( 207 > 

One can then solve for the lower order polynomial coefficients in terms of the 
higher order coefficients by substituting Eqs. (206) and (207) into Eq (205). The result 
specifies polynomial reference trajectory coefficients which satisfy the boundary condi- 
tions. 



®543 “ W 543 



•W. .a. . 

high high 



(208) 



An optimal solution for a polynomial reference trajectory is found by using the 
MATLAB function fminu. This tool numerically finds the solution to an unconstrained 
function minimization problem using a quasi-Newton method The function to be mini- 
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mized is the rate of change of angular momentum, Eq (201), which can be found once a 
reference trajectory is specified. The user makes an initial guess for the higher order refer- 
ence trajectory coefficients. The lower order coefficients are calculated by Eq. (208). The 
MATLAB function then varies the higher order coefficients and recalculates the lower 
order coefficients as necessary to minimize change in angular momentum. One limitation 
to this technique is that the algorithm may converge to a local rather than the global mini- 
mum. 
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III. VALIDATION AND SIMULATION RESULTS 



The computer simulations presented in this chapter were obtained using the MATLAB 
subroutines included in Appendix B. The integrator uses 4 11 ’ and 5 th order Runge-Kutta 
formulas. See Appendix B for documented listings of the computer code. 

A. VALIDATION 

To verify the equations and find the programming bugs, test cases were developed. 
The simulations are analyzed to ensure that universal principles such as conservation of 
energy and angular momentum are not violated. Numeric values for the generic dual two- 
link manipulator system are contained in Table 1. The generic model is the strawman con- 
figuration that all of the test cases are based on with the exception of a few minor varia- 
tions. The variations will be pointed out in the appropriate test cases. The values for the 
generic model’s system properties are picked for uniformity and simplicity. The manipu- 
lator links and the payload are modelled as slender rods of uniform density. 

1. Conservation of Kinetic Energy 

In the first test case, no torques are applied and the initial velocities are nonzero 
Under these conditions, the system links drift subject to the constraints of being pinned 
together. Since potential energy is zero and there are no external energy sources, kinetic 
energy should remain constant. The system begins with the payload parallel to a line 
drawn between the two shoulders and 0.75m away from them. The initial angular rate for 

the centerbody is chosen to be e 0 = 2 deg/sec. The initial angular rate for the payload is d P 
= -5 deg/sec. Initial velocities for the payload center of mass are -0.1 m/sec along the x 
axis and -0.05 m/sec along the y axis. The remaining generalized velocities are calculated 
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TABLE 1. GENERIC MODEL SYSTEM PROPERTIES 





Parameter 


Value 


Length 

(m) 


ko 


0.75 


ki 


0.5 


kj 


0.5 


ko 


0.5 


ki 


0.5 


k: 


0.5 


Ip 


0.75 72 


Mass 

(kg) 


">o 


5 


"'Ll 


1 


777/2 


1 


'"HI 


1 


"'R2 


1 


nip 


1 


Center 

of 

Mass 

(m) 


ko 


0 


kLI 


0.25 


&L2 


0.25 


^Rl 


0.25 


tc R2 


0.25 


tc P 


0.25 


Moments 

of 

Inertia 

(kg-m 2 ) 


>0 


5 


kl 


0.02083 


l L2 


0.02083 


Jri 


0.02083 


Jr 2 


0.02083 


Jp 


0.02083 


Shoulder 

Location 

(deg) 


6 L0 


90 


0 R() 


45 
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based on the values specified for the centerbody and payload. Initial angular rates for the 
manipulator links are b L , = 6.6607 deg/sec, o L2 = -7.0457 deg/sec, b R1 = -2.7553 deg/sec, 

and (» R: = 14.9127 deg/sec. The graphical results from this test case are included in Fig- 
ures 4-8. As indicated in Figure 7, kinetic energy is conserved in this case. 




Figure 4: Test Case 1 Angles 
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Figure 5: Test Case 1 Angular Rates 
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Figure 6: Test Case 1 Time Lapse Stick Figure 
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Figure 7: Test Case 1 Kinetic Energy 
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1.213 



Angular 

Momentum 0.606 

(N-m-sec) 

0 1 

0 5 10 

Time (sec) 

Figure 8: Test Case 1 Angular Momentum 

Test Case 2 is an extension of Test Case 1. This is still a case with nonzero initial 
velocities and no external torques. However, the system geometry is altered to be sym- 
metrical. In addition to conservation of kinetic energy, this test case will ensure that the 
symmetry is preserved. The physical alterations in the system involve moving the loca- 
tion of the left shoulder from 90 degrees to 135 degrees and decreasing the distance from 
the origin to the right shoulder to 0.75 meters. The payload still begins centered between 
the shoulders and parallel to the y axis but is 1.2 m from the origin. To maintain symme- 
try, the initial velocities must also be symmetrical. The initial angular rate for the center- 

body is chosen to be G 0 = 0 deg/sec. The initial angular rate for the payload is also zero. 
Initial velocities for the payload center of mass are zero along the x axis and -0.05 m/sec 
along the y axis. The remaining generalized velocities are again calculated based on the 
values specified for the centerbody and payload. Initial angular rates for the manipulator 

links are 6 L , = 2.3188 deg/sec, o L2 = -7.6851 deg/sec, o R) = -2.3188 deg/sec, and 0 R2 = 
7.6851 deg/sec. This combination of system geometry and initial velocities is designed to 
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cause the payload to drift toward the origin without changing its attitude. Figures 9-13 
show the results from this test case. Kinetic energy is conserved and symmetry is pre- 
served. 
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Figure 9: Test Case 2 Angles 
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Figure 10: Test Case 2 Angular Rates 



50 




Figure 11: Test Case 2 Time Lapse Stick Figure 
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Figure 12: Test Case 2 Kinetic Energy 
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Figure 13: Test Case 2 Angular Momentum 



2. Conservation of Angular Momentum 

As long as a system does not include external torques, one expects that angular 
momentum should be conserved. The joint actuators provide internal torques while the 
reaction wheel is the only external source. Test Cases 1 and 2 did not include a reaction 
wheel and are therefore subject to investigation with respect to conservation of angular 
momentum. Both cases do satisfy the requirement as indicated by Figures 8 and 13. Fur- 
thermore, due to the symmetry in the system in Test Case 2, the angular momentum of the 
left manipulator links should be cancelled out by the angular momentum of the right 
manipulator links. The centerbody and payload should not have any angular momentum. 
Consequently, angular momentum for the system should not only be conserved, it should 
be zero. Figure 13 show that the angular momentum remained virtually zero. The non- 

zero values of about 3x10 are well within the integration algorithm tolerance of 10 . 

Test Case 3 returns to the generic system from Table 1. Initially, the system is at 
rest. Constant torques are applied at both shoulders and nowhere else. The torques are 
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0.01 N-m applied in the positive direction at the right shoulder and the negative direction 
at the left shoulder. Because the joint torques are internal to the system, angular momen- 
tum must still be conserved even though kinetic energy won’t be. Furthermore, since the 
system began at rest, the angular momentum should remain at zero. The results are shown 
in Figures 14-18. Although the angular momentum did not remain identically equal to 

zero, their magnitudes of less than 2x10 are within the 10'° tolerance placed on the inte- 
gration algorithm. 
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Figure 14: Test Case 3 Angles 
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Figure 15: Test Case 3 Angular Rates 




Figure 16: Test Case 3 Time Lapse Stick Figure 
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Figure 17: Test Case 3 Kinetic Energy 




Figure 18: Test Case 3 Angular Momentum 



Test Case 4 is similar to Test Case 3 but the symmetrical system geometry is used 
instead of the generic geometry. This change should produce symmetric motion and zero 
angular momentum. The reaction wheel is still disabled. Figures 19-23 indicate the sys- 
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tern reacted as expected. Changing the torques to time varying profiles rather than con- 
stants led to similar results. 




Time (sec) 

Figure 19: Test Case 4 Angles 




Figure 20: Test Case 4 Angular Rates 
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Figure 21: Test Case 4 Time Lapse Stick Figure 




Figure 22: Test Case 4 Kinetic Energy 
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Figure 23: Test Case 4 Angular Momentum 
3. Wheel Torque and Constraints 

The remaining test cases involved using the reaction wheel on the centerbody. The 
wheel’s function was to maintain attitude pointing. The system begins at rest. The torque 
applied by the wheel is an external torque in this model. Therefore, its value must be the 
same as the change in angular momentum. The wheel torque is found by means of the 
inverse kinematics equations in Chapter II. These calculations are entirely independent of 
finding the change in angular momentum. After a simulation is finished, a separate sec- 
tion in the program code calculates the change in angular momentum using the general- 
ized coordinates, velocities and accelerations produced by the integration. These values 
are plotted along with those of the reaction wheel torque A sample plot is contained in 
Figure 24. This particular plot is for the case of a fifth order polynomial reference trajec- 
tory. The rest of the plots associated with this case are presented later in the Simulations 
section. The validation tests concerning conservation of kinetic energy and angular 
momentum required special circumstances to create those conditions. The requirement 
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that the reaction wheel torque equal the change in angular momentum is more universal. 
It is a verification check performed with every simulation involving a reaction wheel. 




Figure 24: Sample of Wheel Torque and Change in Angular Momentum vs. Time 



An even more universal check also performed with every simulation is the require- 
ment that the constraint equations (Aij + a 0 = o) are satisfied. Figure 25 shows a sample 

plot. This plot was also taken from the fifth order polynomial reference trajectory case. 
The values plotted represent the four constraint equations contained in Eqn 72. The non- 
zero values are attributed to numerical errors created by the integration. 

Finally, a common sense check also performed with every simulation is simply to 
verify that the payload was repositioned to the desired final location. This cannot happen 
if the torques applied to the system were incorrect. This test is a necessary but not suffi- 
cient condition that the code operates correctly. 
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Figure 25: Sample of Constraints vs. Time 



B. SIMULATIONS 

This section presents results from several simulations of an analytical model. The 
desired payload repositioning maneuver is illustrated in Figure 26. The final position for 
the payload involves a 90 degree rotation and the right endpoint finishes where the left 
endpoint started. 

1. Lyapunov Point Controller 

In the first simulation, the repositioning is done entirely by the Lyapunov control- 
ler without the benefit of a reference trajectory. The behavior is that of a point controller 
with an initial displacement rather than that of a tracking controller. Due to the absence of 
the weighted norm reference torques, this controller cannot be consider to have coopera- 
tive nature. Figure 27 presents the angular displacement history. The asterisks on the 
right side of the plot indicate the desired final angles. Although the system is approaching 
the desired final geometry, it has not completely settled down even after 40 seconds. Posi- 
tion errors (Figure 28) are still present as well as nonzero velocities (Figure 29). Also note 
that the reaction wheel torque is quite high during the maneuver (Figure 30). The joint 
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actuator torques are considerably less than the reaction wheel torque. They are not identi- 
fied individually because the most important feature of Figure 30 is the reaction wheel 

torque. As a quantitative measure of this controller’s quality, jju wh |dt produces a value of 

17.3841. The oscillatory nature of the system is evident in the angular position and veloc- 
ity plots. Despite the oscillations, however, the stability of the controller is also illus- 
trated. Figure 31 depicts the system geometry at several instances during the maneuver. 
The left manipulator links actually cross over each other. In experimental hardware, the 
links would collide instead. Figure 32 removes the clutter that is present in Figure 3 1 and 
displays only the initial and final geometry. The Lyapunov point controller also does a 
poor job of maintaining the centerbody attitude. This is clearly evident in Figures 27 and 
3 1 . The attitude error peaks at about 1 6 degrees. 




Figure 26: Desired Repositioning Maneuver 
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Figure 27: Lyapunov Point Controller Angles 



Displacement Errors vs l ime 




Figure 28: Lyapunov Point Controller Displacement Errors 
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Theta Dots vs Time 




Figure 29: Lyapunov Point Controller Angular Rates 
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Command Torques vs l ime 




Figure 30: Lyapunov Point Controller Command Torques 
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Figure 31: Lyapunov Point Controller Time Lapse Stick Figure 
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Figure 32: Lyapunov Point Controller Initial and Final Stick Figures 
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2. Lyapunov Tracking Controller 



This controller uses the following equation to calculate control torques 



u . C t (-K v s<j + C, u ref - <C 2 q-C 2 q ref ) - <C, - C,J - K,,5q } 



(209) 



This equation was developed in the analytical chapter and repeated here for conve- 
nience. The command torques are based on errors with a reference trajectory. Reference 
torques which resulted from minimizing a weighted norm of the actuator torques associ- 
ated with the reference trajectory are also included. 
a. 5' 1 ' Order Reference Trajectoiy 

In this simulation, a fifth order polynomial reference trajectory is applied to the 
payload generalized coordinates. The payload coordinates displacements, velocities, and 
accelerations resulting from this polynomial are depicted in Figure 33. When calculating 
the reference torques from the inverse kinematics, the six joint actuators are all weighted 
equally. The maneuver time is selected to last 10 seconds. As is demonstrated in Figures 
34-36, the system successfully moves from initial conditions to desired final conditions. 
The displacement errors are less than 10“ 4 deg (Figure 34). The command torques (Figure 
37) are an order of magnitude smaller than for the previous simulation which lacked a 
reference trajectory. Evaluating J|u wh |di leads to the dramatically improved value of 
0.5746. More importantly, the centerbody attitude is maintained throughout the maneuver. 
Figure 38 shows the time lapse depiction of the maneuver. 
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Displacement vs Normalised Time 






Figure 33: 5 111 Order Reference Trajectories 
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Figure 34: S 11 ' Order Angles 
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Displacement Frrors vs Time 




Figure 35: 5*** Order Displacement Errors 



ThetaDols vs Time 




Figure 36: 5 Order Angular Rates 
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Figure 37: 5 th Order Command Torques 




Figure 38: 5 th Order Time Lapse Stick Figure 
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b. X th Order Reference Trajectory 

By increasing the order of the reference trajectory polynomial while 
maintaining the same boundary conditions concerning velocity and acceleration, one 
hopes to achieve improved performance. For example, the domain of all sixth order 
polynomial functions includes all fifth order polynomial functions as a subset. Therefore, 
when searching all sixth order polynomials for coefficients which will minimize the cost 
function, one possible solution is the fifth order polynomial already used. Using the 
function minimization routine discussed in the previous chapter, a sixth order polynomial 
function was found Although there was some improvement, the change in performance 
was not significant. The same was true for a seventh order function. An eighth order 
function is presented here. It was hoped that the increased order would be enough of a 
departure from the fifth order cause to produce significant improvement in reducing the 
centerbody disturbance torque. The algorithm converged to a solution for the eighth order 
polynomial after running approximately two hours on a personal computer with an Intel 
486-DX50 cpu. The resulting trajectories are very similar to those for the fifth order case 
and are displayed in Figure 39. The most obvious difference is a lack of symmetry. Plots 
for this case are contained in Figures 40-43. The value of J|u wh |dt for this case was 0.5705. 
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Displaccmert vs Nomraltfed Time 




Figure 39: 8 tl! Order Reference Trajectories 
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Figure 40: 8 th Order Angles 
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ThetaDots vs Time 




Figure 41: 8 th Order Angular Rates 




Figure 42: 8 <h Order Command Torques 
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Figure 43: 8 th Order Time Lapse Stick Figure 



3. Modified Lyapunov Tracking Controller 

This simulation represents a compromise between the Lyapunov point controller 
and the Lyapunov tracking controller. Because the Lyapunov point controller does not use 
a reference trajectory, the cost function which minimizes the weighted norm of the actua- 
tor torques is completely bypassed. The modified Lyapunov controller removes the refer- 
ence torque term from the command torque calculation (Eqn 209) but calculates command 
torques based on errors with a reference trajectory. Like the Lyapunov point controller, 
the modified Lyapunov tracking controller does not minimize a weighted norm of the 
actuator torques and is therefore not a cooperative controller. The angle histories in Figure 
44 exhibit less of the oscillatory nature than the point controller simulation, but the accu- 
racy shown in Figure 45 is considerably worse than the reference trajectory simulations. 
Figures 46-48 also illustrate behavior better than the point controller but not as good as 
when command torques are found using Eqn 209. The magnitude of the command torques 
show an order of magnitude improvement over the point controller. This is directly attrib- 
utable to using intermediate reference points on the way to a desired final state rather than 
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attempting to achieve the desired final state all at once. Calculating j|u wh |di produced a 

value of 2.4523. The time lapse figure shows that the motion is much less wild but the 
centerbody attitude error is still noticeable. 




0 20 40 

Time (sec) 

Figure 44: Modified Lyapunov Tracking Controller Angles 
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Figure 45: Modified Lyapunov Tracking Controller Displacement Errors 
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ThctaDots vs l ime 




Figure 46: Modified Lyapunov Tracking Controller Angular Rates 
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Figure 47: Modified Lyapunov Tracking Controller Command Torques 
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Figure 48: Modified Lyapunov Tracking Controller Time Lapse Stick Figure 
4. Comparison of Controllers 

Table 2 summarizes the results of the Lyapunov point controller, the two Lyapunov 
tracking controller cases, and the modified Lyapunov tracking controller. The point con- 
troller clearly has the worst performance with high reaction wheel torque and large center- 
body attitude error. The tracking controller performs much better. Reaction wheel torque 
is greatly reduced and centerbody attitude error is eliminated As expected, increasing the 
order of the polynomial reduces the reaction wheel torque further, but the improvement is 
relatively small. The modified tracking controller strikes a compromise between the point 
controller and the tracking controller. 
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TABLE 2. COMPARISON OF HYPOTHETICAL MODEL 
SIMULATIONS 





KJ d » 


K..I 


Centerbody 
Attitude 
Error (deg) 


Cooperative 


Point Controller 


17.3841 


2.9365 


16.2261 


No 


Tracking 

Controller 


5 th Order 


0.5746 


0.0961 


0.0000 


Yes 


8 Ul Order 


0.5705 


0.0885 


0.0000 


Yes 


Modified Tracking 
Controller 


2.4523 


0.3950 


1.1910 


No 
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IV. EXPERIMENTAL WORK 



The experimental phase of this research was conducted on the Spacecraft Robotics 
Simulator (SRS). The SRS is a derivative of the Flexible Spacecraft Simulator (FSS) ini- 
tially developed by Watkins [Ref 17] and later modified by Hailey [Ref 18]. Sorensen 
[Ref 1 8] began the work to convert the FSS into the SRS. 

A. SETUP 

The SRS permits experimental investigation of two dimensional robotics motion and 
rotational spacecraft dynamics. The SRS is illustrated in Figures 49 and 50. The simula- 
tor hardware is floated on an eight foot by six foot granite table by means of a thin layer of 
air supplied by an external source. The table is polished to within 0.001 inch peak to val- 
ley and leveled to prevent gravitational accelerations from impacting the motion across its 
surface. The following sections describe the simulated spacecraft with its associated sen- 
sors and actuators and the controller which together form the SRS. The spacecraft compo- 
nents are the centerbody, two manipulators, and a payload. 
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Figure 49: Spacecraft Robotics Simulator 
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Figure 50: System Top View 



1. Centerbody 

The centerbody is a 30 inch diameter, 7/8 inch thick aluminum disk. The center- 
body carries a position sensor, rate sensor, momentum wheel, thrusters, and an air tank to 
power the thrusters. The centerbody also serves as the mounting platform for the manipu- 
lators. The centerbody is floated by a central air bearing and three air pads located at 120 
degree intervals near the outer edge. The air pads are each capable of floating 60 pounds 
when the air pressure supplied to the pads is 80 psi. The air bearing is attached to an over- 
head I-beam which restricts to motion of the centerbody to rotation only. 
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Centerbody angular position is sensed by a Rotary Variable Displacement Trans- 
ducer (RVDT) mounted directly above the air bearing. The RVDT is a model R30D man- 
ufactured by Schaevitz Sensing Systems. Its linear range is restricted to ± 40 degrees. 
Centerbody angular rate is measured by a rate transducer manufactured by Humphrey, Inc. 
The instrument has a range of ±100 deg/sec and a minimum threshold of 0.01 deg/sec. 

Centerbody angular position is controlled by a momentum wheel. The momentum 
wheel speed is measured by a tachometer contained in the servo motor which drives the 
momentum wheel. The centerbody momentum wheel is powered by a model JR16M4CH/ 
F9T servo motor manufactured by PMI Industries. Characteristics of this motor are sum- 
marized in Table 3. Although the centerbody also carries two thrusters, they are not used 
in this research. 



TABLE 3. MOMENTUM WHEEL MOTOR 
CHARACTERISTICS 



Manufacturer 


PMI Industries 


Model 


JR16M4CH/F9T 


Rated Output Speed (rpm) 


3000 


Rated Current (amps) 


7.79 


Rated Voltage (volts) 


168 


Rated Torque (in-lb) 


31.85 


Height (in) 


4.5 


Weight (lb) 


17.5 


Outside Diameter (in) 


7.4 



2. Manipulators 

Two two-link manipulators are mounted 60 degrees apart on the centerbody. Each 
manipulator has three joints. The shoulder joints are supported by the centerbody while 
the elbow and wrist joints are supported by two air pads apiece. The links between the 
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joints are stifT laterally but permit some flexibility vertically. This feature increases the 
tolerances on the air pad height adjustment. 

Left arm joint angles are measured by the same model RVDT as is used on the cen- 
terbody. All three of the left arm actuators are series 9FGHD servo disk motors manufac- 
tured by PM1 Industries. Joint angles on the right arm are sensed by encoders purchased 
with the joint actuators. The encoder resolution is 0.005 degrees. The right arm joint 
actuators arm are harmonic drive dc servo actuators manufactured by HD Systems, Inc. 
The shoulder actuator is model RFS-25-6018-E036AL while the elbow and wrist actua- 
tors are model RFS-20-60I2-E036AL. Specifications for the three types of joint actuators 
are contained in Table 4. 




Figure 51: Left Manipulator Top and Side Views 
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Figure 52: Right Manipulator Top and Side Views 



TABLE 4. MANIPULATOR ACTUATOR CHARACTERISTICS 



Manufacturer 


HD Systems 


HD Systems 


PMI Industries 


Model 


RFS-25-6012 


RFS-25-6018 


9FGHD 


Reduction Ratio 


1:50 


1:50 


1:148.5 


Rated Output Speed (rpm) 


60 


60 


17 


Rated Current (amps) 


2.9 


3.9 


5.6 


Rated Voltage (volts) 


75 


75 


12 


Rated Torque (in-lb) 


174 


260 


80 


Height (in) 


8.8 


9.6 


3 


Weight (lb) 


9.3 


14.1 


3.2 


Footprint (in) 


4.3 (1) 


5.1 (1) 


4.8^ 2) 



1 Side of square 

2 Diameter of eircle 
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The joint actuators are all driven by Kepco power supplies. These bipolar, pro- 
grammable, linear amplifiers can be controlled manually from the front panel or con- 
trolled remotely with a ±10 volt signal. In this application, the power supplies are 
operated in the current control mode with the voltage and current limits manually set con- 
sistent with the values in Table 4. The specific power supply models and their characteris- 
tics are summarized in Table 5. 



TABLE 5. POWER SUPPLIES CHARACTERISTICS 



Model 


BOP 72-6M 


BOP 72-3M 


BOP 20-10M 


Actuators Controlled 


Right Shoulder 


Right Elbow, 
Right Wrist 


All Left Arm 
Joints 


DC Output Range 


±72 volts 
±6 amps 


±72 volts 
±3 amps 


±20 volts 
±10 amps 


Closed Loop Gain 


0.6 (amp/volt) 


0.3 (amp/volt) 


1 .0 (amp/volt) 



3. Payload 

The payload is a rigid bar mechanically fastened to the ends of both manipulators. 
The payload is supported entirely by the air pads on the manipulator wrist joints. Ballast 
can be added to the payload to change the mass and inertia characteristics of the system. 
This allows for the construction of cases in which the mass of the payload is nontrivial 
compared to the spacecraft centerbody. The payload contains no sensors or actuators. 
Payload position is derived from the manipulator joint angles. 

4. Controller 

The AC-100 programmable controller manufactured by Integrated Systems, Inc. 
controls the SRS. The AC-100 includes an Intel 80386 Application Processor, an Intel 
80386 Multibus II Input/Output Processor, an Intel 80386 Communication Processor, and 
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Intel 80387 Coprocessor, a Weitek 3167 Coprocessor, and Analog-To-Digital and Digital- 
To-Analog Data Translation DT2402 Input/Output Board, two INX-04 Encoder and Digi- 
tal -To- An al og Servo Boards, and an Ethernet Interface Module The AC-100 also 
includes software installed on a VAX 3100 Series Model 30 workstation. The software 
permits design of a controller in block diagram graphical form and conversion of the dia- 
gram to C language programming code. The user is also able to design an interactive ani- 
mation window to operate the controller. The AC-100 receives input signals from the 
sensors and the graphical user interface. AC-100 output signals go to the power supplies 
driving the actuators or to the graphical user interface for display. 

5. System Integration 

The differences between the ideal world of an analytical simulation and the real 
world of actual hardware became apparent during system integration. A few problems 
arose then requiring some modification of the experiment. The first problem concerned 
floating the centerbody. It exhibited a noticeable resistance to rotation. This is due in part 
to the air pressure of the available air supply. Because it was only 40 psi, the air pads per- 
formance was degraded by a factor of two. Prior to mounting the manipulators, the cen- 
terbody weighed approximately 125 lbs. Adding the shoulder motors increased the 
centerbody weight to 145 lbs. The extra weight may have been enough to overwhelmed 
the centerbody air pads. A second contributing factor to the centerbody drag is the inabil- 
ity of the central air bearing to function except under very low lateral loading. The modi- 
fication to the experiment created by the centerbody problem is to not float the centerbody. 

A second problem involved using the RVDTs. As envisioned, the experiment 
requires one RVDT for the centerbody and three for the left manipulator joints. The Space 
Dynamics laboratory has a total of three in stock Although a fourth has been ordered, it 
did not arrive in time to be used. Using the existing RVDTs revealed another problem. 
Data acquisition of the RVDT signal by the AC-100 exhibited a random toggling of the 
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sensed value between a good reading and a value of zero. Because the angle information 
is critical to calculating actuator commands, this behavior is unacceptable. Consultation 
with the Integrated Systems technical support group revealed that this type of behavior is a 
bug within the AC- 100 software which has been corrected in more recent versions. Use of 
the newer version was not possible because it requires upgrading the VAX workstation 
hardware and an updated version of the VMS operating system The experimental modifi- 
cation used to overcome these difficulties is to derive the joint angles and velocities of the 
left manipulator by using the sensed information from the right manipulator encoders. 
Velocities were not sensed directly but approximated by the change in displacement which 
occurred since the last sample divided by the sample rate 

A third obstacle involved the limitations of software to design the control algo- 
rithm. The block diagram construction method did not permit convenient matrix opera- 
tions. Matrix multiplication must be programmed in an element by element basis. Matrix 
inversion must also be calculated by constructing a series of blocks to find each element. 
This handicap is not serious when the system equations of motion are of low order. How- 
ever, the dual two link manipulator configuration is eighth order and beyond the practical 
means of programming complex matrix operations, especially matrix inverses. Recall that 
the command torques are calculated by the following relationship 




( 210 ) 



When the differences between the actual path and the reference path are small, this control 
law simplifies to something very similar to a PD controller. Therefore, the control law 
used by the experiment is a PD controller rather than the complete Lyapunov controller. 

Performance differences between the left and right manipulator actuators also pre- 
sented some problems. Because of the actuator redundancy, any three joint actuators 
should be enough to follow a reference trajectory. This fact can be demonstrated by using 
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only the three right joint actuators. However, the same trajectory is not possible with only 
the left actuators. The torque provided by the left joint actuators is insufficient to com- 
pletely overcome the internal friction of the right joint actuators. Even when the left joint 
actuators are commanded manually from the front panel, there is no correction to reduce 
the position error. When steadily increasing the commanded current to the motor, the cur- 
rent limit is reached before the motor responds. 

B. RESULTS 

The reference trajectory for the experimental phase is slightly different from that used 
in the analytical section. The reference maneuver still involves a 90 degree rotation of the 
payload with the right endpoint ending where the left endpoint began. The differences 
arise from the system parameter such as lengths and masses not being the same as in the 
generic hypothetical model. The desired reference maneuver is depicted in Figure 53. 
Results are shown in Figures 54-58 and summarized in Table 6. The sudden changes from 
believable values to zero in the figures are problems with the data acquisition software and 
do not indicate actual changes in the experimental hardware geometry. 




Figure 53: Desired Experimental Repositioning Maneuver 
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Figure 54: 0p Commanded, Actual, and Error Angles vs Time 
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Figure 55: G^j Commanded, Actual, and Error Angles vs Time 
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Figure 56: 0 L2 Commanded, Actual, and Error Angles vs Time 
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Figure 57: 0m Commanded, Actual, and Error Angles vs Time 
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Figure 58: 0^2 Commanded, Actual, and Error Angles vs Time 
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TABLE 6. EXPERIMENTAL ERROR ANGLES 





Errors (deg) 


Initial 


Final 


Maximum 

Magnitude 


9p 


0.2550 


-0.3383 


0.5527 


On 


-0.4574 


0.0366 


0.7797 


0 L2 


0.0225 


0.1873 


0.3035 


9ri 


0.1037 


-0.0808 


0.1628 


9r2 


0.3350 


0.3950 


0.7742 
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V. SUMMARY AND CONCLUSIONS 



A. SUMMARY 

The dynamics of a dual two-link manipulator system which is repositioning an already 
grasped payload have been analyzed The equations of motion for the system were devel- 
oped using Lagrange’s method. The resulting equations were highly nonlinear, coupled, 
second order differential equations. Given any reference trajectory, the actuator torques 
that will produce that trajectory were calculated to minimize a weighted norm of the 
torques. Stability of the system during the repositioning maneuver was ensured by a con- 
troller derived from Lyapunov stability theory. Equations for deriving joint angles from 
centerbody and payload reference values was also developed. Polynomial reference tra- 
jectories were presented as an attractive means to specify a reference trajectory. 

The analytical model was validated using test cases in which some results could be 
predicted in advance. The model demonstrated conservation of energy when no torques 
were applied. It also exhibited conservation of angular momentum whenever the reaction 
wheel was disabled. The model also maintained symmetric geometry in the appropriate 
test cases. In cases which used the reaction wheel, conservation of energy and angular 
momentum did not apply. However, comparison of the change in angular momentum with 
the reaction wheel torque provided validation. Finally, in all test cases as well as simula- 
tions, the constraints were satisfied as measured by Acj + A 0 = o. 

Results from simulations indicated that the Lyapunov point controller, although stable, 
behaved poorly. Large centerbody attitude errors, high command torques, and wild oscil- 
lations make this controller undesirable for large repositioning maneuvers. The Lyapunov 
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tracking controller exhibited dramatic a improvement in performance. Centerbody atti- 
tude errors were removed and reaction wheel torque decreased significantly. 

The experimental phase revealed that the controller required further simplification for 
compatibility with the laboratory resources. Acceptable results were obtained using a PD 
control law with a reference trajectory. 

The objectives of this research were to 1) develop a stable control law that facilitates 
cooperation among the manipulators as they reposition the payload, 2) minimize the joint 
actuator effort, 3) reduce the disturbance torque transmitted to the spacecraft main body 
by the manipulator motion, and 4) validate the analytical development with experimental 
results. The Lyapunov controller satisfies the first objective. The second objective is 
achieved by the weighted norm calculation of the actuator torques. Reduction of the cen- 
terbody disturbance torque is accomplished through reference trajectoiy selection. 
Although a rigorous application of classical optimal control techniques proved impracti- 
cal, a polynomial reference trajectory in which the coefficients were selected to reduce the 
disturbance torque was easily applied. Difficulties were encountered with regards to the 
fourth objective, experimental work. The controller developed analytically could not be 
directly transferred to the laboratory. This was due to a combination of hardware limita- 
tions and real world conditions instead of the ideal environment of the analytical model. 
The controller was adapted to the realities of the laboratory and resulted in successful 
accomplishment of a payload repositioning maneuver. 

B. ORIGINAL CONTRIBUTIONS 

A simulation tool has been developed to analyze the dynamics of a space based robot- 
ics system. Some of the features of this tool include: 
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(i) rotational motion of the spacecraft centerbody and planar motion of the manip- 
ulators and payload; 

(ii) minimization of a weighted norm of the actuator torques based on a user sup- 
plied weighting matrix; 

(iii) calculation of polynomial reference trajectory coefficients to produce a local 
minimum for the integral of the absolute value of the disturbance torque based 
on a user supplied order for the reference polynomial and an initial guess for 
the coefficients; 

(iv) a reference trajectory with zero velocity and acceleration at the beginning and 
end of the maneuver; 

(v) a Lyapunov controller which guarantees stability in the face of perturbations 
between the reference trajectory and the actual dynamics caused by errors in 
the initial conditions. 

An experimental test bed was also developed. This effort involved the design of the 
manipulator components and the development of a real time controller. This test bed 
remains in the Spacecraft Dynamics and Control Laboratory and is available for follow-on 
work. 

C. RECOMMENDATIONS FOR FURTHER STUDY 

As with any research, this work answers some questions but raises others. One of the 
areas that could receive further attention is the selection of the Lyapunov controller gains. 
The theory requires positive definite matrices to ensure stability but offers no insights con- 
cerning selection of the matrices to improve performance. For any given set of controller 
matrices, one expects the relative merits of the point controller, tracking controller, and 
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modified tracking controller to remain the same. However, still better performance might 
be achieved across the board if the gains were optimized. 

Rather than merely changing Lyapunov controller gains, one might investigate another 
Lyapunov controller by beginning with a different candidate Lyapunov function than the 
one presented here. The choices are infinite and the results and performance difficult to 
predict. 

Trajectory optimization is another area that would benefit from further work. The 
function minimization algorithm used to select polynomial coefficients converged to local 
minima solutions depending on the initial guess for the coefficients. The search for a glo- 
bal minimum for a particular order polynomial requires further investigation. An alternate 
approach with respect to trajectory optimization is to use some function other than a sim- 
ple polynomial to describe the trajectory. Possible trajectories might be TchebychefF poly- 
nomials, Legendre polynomials, or Fourier series. 

To help bridge the gap between the analytical model and the real world hardware, one 
could consider modifying the controller to include joint friction, actuator backlash, sensor 
noise, and flexibility. One could also consider using a minimum generalized coordinate 
formulation. One might also attack the differences from the hardware perspective by 
seeking components that more closely resemble those in the analytical model. Another 
improvement in the experiment would be to replace the existing joint velocity approxima- 
tions with either an observer or an actually velocity measurement. 

Finally, it’s a three dimensional world. Extending the analytical model and, if possi- 
ble, the laboratory experiment to include out of plane motion should be considered. 
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APPENDIX A: EXPERIMENTAL CONTROL BLOCK 

DIAGRAMS 



This appendix includes the block diagrams of the System Build super blocks made to 
control the SRS. The heirarchy among the super blocks is illustrated in Figure 59. Both is 
the parent superblock. The ohters are lower level super blocks 



Both 

Parameters 

Trajectories 

Encoders 

LeftAngles 

Parti 

Controller 

Part2 

Part3 

Figure 59: Super Blocks Hierarchy 

The block diagram for super block Both is shown in Figure 60. Inputs into the dia- 
gram include the sensor signals from the hardware and user operated dials to select the 
controller gains and enable switches which select the combination of joint actuators to 
enable. The outputs include commanded, reference, and error signals for each of the cen- 
terbody angle, joint angles, and payload angle. Motor current commands to the Kepco 
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power supplies are also outputs. Block 56 contains the system parameter values for the 
experimental hardware. This block is expanded and displayed in Figure 61. Block 8 con- 
tains the position and velocity values for a reference trajectory in a look-up table. It also 
contains a table to reset the system back to its original geometry to permit rerunning the 
reference trajectory. This block is expanded in Figure 62. Conversion of the encoder 
pulses from the right manipulator into angle and angle rate information is done in Block 7 
which is expanded in Figure 64. Conversion of the encoder pulses from the right manipu- 
lator into angle information for the left manipulator is done in Block 49. Details of this 
block are shown in Figures 64-64. The PD controllers which convert the error signals into 
actuator commands are in Block 40. This block is expanded in two parts. The actuator 
commands for the right and left manipulator are shown in Figures 68 and 69 respectively. 
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Discrete Super-Block Sampling Interval rirst Sample Ext. Inputs Ext. Outputs Enable 
both 0.0100 0. 48 25 Parent 



0 

I 

a 





Figure 60: Overall Control Block Diagram 
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Discrete Super-Block Sampling Interval First Sample Ext. Inputs Ext. Outputs Enable 

Parameters 0 . 01 00 0 . 0 28 Parent 




Figure 61 : Parameters Block Diagram 
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Figure 62: Reference Trajectory Block Diagram 
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Discrete Super-Block Sampling Interval First Sample Ert. Inputs Ext. Outputs Enable 
Encoders 0.0100 0. 3 6 Parent 




Figure 63: Encoders Block Diagram 
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Figure 64: Left Angles Block Diagram 
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27-JUL-93 




Figure 65: Part 1 Block Diagram 
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Discrete Super-BlocJc Sampling Interval First Sample Ext. Inputs Ext. Outputs Enable 
Part 2 0.0100 0. 8 4 Parent 



CO 




Figure 66: Part 2 Block Diagram 
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Discrete Super-Block Sampling Interval First Sample Ext. Inputs Ext. Outputs Enable 
Part3 0.0100 0. 19 3 Parent 




Figure 67: Part 3 Block Diagram 
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tn*us *• 03*07 



Discrete Super-Block Sampling Interval First Sample Ext. Inputs Ext. Outputs Enable 
Controller 0.0100 0. 53 12 Parent 




Figure 68: Right Manipulator Controller Block Diagram 
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Discrete Super-Block Sampling Interval First Sample Ext. Inputs Ext. Outputs Enable 

Controller 0.0100 0. 53 12 Parent 



m 




Figure 69: Left Manipulator Controller Block Diagram 
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APPENDIX B: MATLAB CODE 



The following listings are the MATLAB code used for the analytical simulations. The 
modules are included in alphabetical order. The hierarchical relationship between the 
modules is illustrated in Figure 70. The integration modules ode2 and odemin are minor 
variants of the MATLAB supplied module ode45. The modifications permit more param- 
eters to be passed to and from these modules without having to include the extra variables 
in the state vector. Similarly, fminu2 modified the MATLAB unconstrained function min- 
imization module, fminu, to include some diagnostic statements while running. 



MainOpt 

fminu2 

MainMin . 

odemin 



Main2 



RefMin2 



ode2 — 

Draw3 

AngMo 

Matx 

MatxFix 



AngMo2 



Eqn2- 



Ref2 — 
Matx 
MatxFix 
AngMo2 



Matx 

MatxFix 

AngMo2 



Figure 70: MATLAB Modules Hierarchy 
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A. AngMo 



% Filename is "AngMo.m" 

% This file calculates the angular momentum of the system 
function [Hs] = AngMo(Ls,Ms,CMs,Is,Q,Qdot) 

% OUTPUT: 

% Hs = 1x7 row vector of angular velocities. The first element is for 
% the eenterbody. The next four elements arc for the left upper 

% and lower arm followed by the right upper and lower arm The 

% last two elements are for the payload and a total of all the 

% previous elements. [HO HL1 HL2 HR1 HR2 1 IP UTotal] 

% 

% INPUT: 

% Ls = 7x 1 vector of lengths (m) 

% 1 st element = distance from origin to left arm mount 

% 2nd & 3rd elements wrt left arm (from shoulder toward wrist) 

% 4th element = payload length 

% 5th & 6th elements wrt right arm (from wrist toward shoulder) 

% 7th element = distance from right arm mount to origin 

% [LO; L 1 ; L2; LP; R2; R 1 ; RO] 

% Ms = 6x1 column vector containing the masses (kg) 

% 1 st element = mass of spacecraft eenterbody 

% 2nd & 3rd elements = mass of left arm (upper arm then lower aim) 
% 4th & 5th elements = mass of right arm (upper arm then lower arm) 
% 6th element = payload mass 
% [MO; ML 1 ; ML2; MR 1 ; MR2; MP] 

% CMs = 6x1 column vector containing center of mass locations 
% [LcO; LcLl; LcL2; LcRl ; LcR2; LcP] 

% ls = 6x 1 column vector containing the moments of inertias about the 
% respective body's center of mass (kg m A 2) 

% 1st element = inertia of spacecraft eenterbody 
% 2nd & 3rd elements = inertia of left ami (upper aim then lower aim) 
% 4th & 5th elements = inertia of right arm (upper arm then lower arm) 
% 6th element = payload inertia 
% [10; IL 1 ; IL2; IR1 ; IR2; IP] 

% Q = 8x1 column vector of generalized coordinates 
% Qdot = 8x1 vector of generalized velocities 



%%%%%%%%%%%% 0 /o 0 /o% 0 /o% 0 /o 0 /o% 0 /o 0 /o 0 /o%%%%%%%% 

%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Lengths (m) 

L0 = Ls(l); 

LI =Ls(2); 

L2 = Ls(3); 

LP = Ls(4); 

R2 = Ls(5); 

R1 = Ls(6); 

RO = Ls(7), 

% Member masses (kg) 

MO = Ms(l); 

ML1 = Ms(2); 

ML2 = Ms(3); 

MR1 = Ms(4); 

MR2 = Ms(5); 

MP = Ms(6); 

% Center of mass distances (m) 
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LcO = CMs(l); 

LcL I = CMs(2); 

Lcl .2 = CMs(3); 

LcR I = CMs(4); 

LcR2 = CMs(5); 

LcR = CMs(6); %mcasured from left end 

% MOI about center of mass 

10 = Is(l ); 

11 I = Is(2); 

IL2 = Is(3); 

IR1 = Is(4); 

IR2 = Is(5); 

IP = Is(6), 



% Coordinates (rad & m) 

ThO = Q( I ); 

Till. I = Q(2), 

Thl.2 = Q(3); 

ThRI = Q(4); 

ThR2 = Q(5); 

ThP = Q(6); 

XP = Q(7); 

YP = Q(8); 

% Coordinate Rates (rad/sec & m/sec) 

ThOd = Qdot( 1 ); 

Thl. Id = Qdot(2); 

Till, 2d = Qdol(3); 

ThR Id = Qdot(4); 

ThR2d = Qdot(5); 

ThPd = Qdol(6): 

XPd = Qdol(7), 

YPd = Qdol(8); 

% Angular Momentum 
1 10 = Th0d*(I0 + M0*Lc0 A 2); 

1 1L 1 = ThOd*(lL 1 +ML, 1 *(L0 A 2+LcL 1 A 2+2*LO*Lcl. I *cos(ThI, 1 ))) + . 

ThL 1 d*(IL 1 +MI. 1 *(LcL 1 A 2+L0*LcL 1 *cos(TliL 1 ))). 

ML2 = ThOd*(IL2+ML2*(LO A 2+Ll A 2+LcL2 A 2+2*LO*LI*cos(ThI,l) + ... 
2*L 1 *LcL2*cos(ThL2)+2*L0*l.cI.2*cos(Tlil. 1 +ThL2))) + ... 
ThLld*(IL2+MI.2*(Ll A 2+LcL2 A 2+LO*L l*eos('flil.l) + ... 
2*Ll*LcL2*eos(ThL2)+LO*LcL2*cos(Tlil,l + flil.2))) + ... 
ThL2d*(IL2+MI.2*(LcL2 A 2+Ll*LcL2*cos(ThL2) + ... 
L0*LcI.2*cos(ThL 1+Thl.2))); 

HR 1 = ThOd*(IR 1 +MR1 *(R0 A 2+LcR 1 A 2+2*RO*LcR 1 *cos(ThR 1))) + ... 

ThR 1 d*(IR 1 +MR 1 *(LcR 1 A 2+R0*LcR 1 *cos(ThR 1 ))); 

1 1R2 = ThOd*(IR2+MR2*(RO A 2+Rl A 2+LcR2 A 2+2*RO*R 1 *cos(ThR 1 ) + ... 
2*R1 *LcR2*cos(ThR2)+2*RO*LcR2*cos(TliR l+ThR2))) + ... 
ThRld*(IR2+MR2*(R I A 2+LcR2 A 2+RO*R I *cos(TliR 1 ) + ... 
2*Rl*LcR2*cos(ThR2)+RO*LcR2*cos(TliRl +ThR2))) + ... 
ThR2d*(IR2+MR2*(I.cR2 A 2+Rl *LcR2*cos(ThR2) + ... 
R0*LcR2*cos(ThR 1 +ThR2))); 

IIP = ThPd*IP + MP*(-XPd*YP + YPd*XP); 

I ITolal = HO + I1L1 +HL2 + HR1 + HR2 + I IP, 

I Is = (HO HI, I HL2IIRI HR2 IIP I ITolal]; 
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B. AngMo2 



% Filename is H AngMo2.m" 

% This file calculates the angular momentum of the system 
% Version 2 also finds the rate of change of angular momentum 
function [Hs, Hdots] = AngMo2(Ls,Ms,CMs,ls,Q,Qdot,Qddot) 

% OUTPUT: 

% Hs = 1\7 row vector of angular velocities. The first element is for 
% the centerbody. The next four elements are for the left upper 

% and lower arm followed by the right upper and lower arm. The 

% last two elements are for the payload and a total of all the 

% previous elements. [HO HL 1 HL2HR1 11R2 IIP llTotal] 

% Hdots = 1 x7 row vector of the change in angular velocities. I he order 
% is the same as for 1 Is 
% 

% INPUT: 

% Ls = 7x1 vector of lengths (m) 

% 1 st element = distance from origin to left arm mount 

% 2nd & 3rd elements wrt left arm (from shoulder toward wrist) 

% 4th element = payload length 

% 5th & 6th elements wrt right arm (from wrist toward shoulder) 

% 7 tli element = distance from right arm mount to origin 

% [LO; LI; L2; LP; R2; Rl; ROJ 
% Ms = 6x1 column vector containing the masses (kg) 

% 1 st element = mass of spacecraft centerbody 

% 2nd & 3rd elements = mass of left arm (upper arm then lower arm) 
% 4th & 5th elements = mass of right arm (upper arm then lower arm) 
% 6th element = payload mass 
% [MO; ML 1 ; ML 2; MR 1 ; MR2; MP] 

% CMs = 6x1 column vector containing center of mass locations 
% [LcO, LcLl; LcL2; LcRl ; LcR2; LcP] 

% Is = 6x1 column vector containing the moments of inertias about the 
% respective body's center of mass (kg m A 2) 

% 1 st element = inertia of spacecraft centerbody 

% 2nd & 3rd elements = inertia of left arm (upper arm then lower aim) 
% 4th & 5th elements = inertia of right arm (upper arm then lower aim) 
% 6th element = payload inertia 
% [10; IL 1 ; IL2, IR 1 ; 1R2; IP] 

% Q = 8x1 column vector of generalized coordinates 
% Qdot = 8x1 vector of generalized velocities 
% Qddot = 8x1 vector of generalized accelerations 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Lengths (m) 

LO = Ls(l); 

LI = Ls(2); 

L2 = Ls(3); 

LP = Ls(4); 

R2 = Ls(5); 

Rl = Ls(6); 

R0 = Ls(7); 

% Member masses (kg) 

MO = Ms(l); 

ML1 = Ms(2); 

ML2 = Ms(3); 

MR1 = Ms(4); 
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MR2 = Ms(5); 

MP = Ms(6), 

% Center of mass distances (m) 
l.cO =CMs(l); 

LcLl = CMs(2), 

I,cL2 = CMs(3); 

LcRI = CMs(4), 

I, cR2 = CMs(5), 

LcP = CMs(6); %measured from left end 

% MOl about center of mass 
10 =Is(l); 

II, I = ls(2); 

1L2 = ls(3); 

IR1 = Is(4); 

1R2 = ls(5); 

1I > = ls(6); 



% Coordinates (rad & m) 

ThO =Q(1); 

ThL 1 = Q(2); 

ThL2 = Q(3); 

ThRl = Q(4); 

ThR2 = Q(5); 

ThP =Q(6); 

XP = Q(7); 

YP = Q(8); 

% Coordinate Rates (rad/sec & m/sec) 

ThOd = Qdot(l); 

ThL Id = Qdot(2); 

ThL 2d = Qdot(3); 

ThRld = Qdot(4); 

ThR2d = Qdot(5); 

ThPd = Qdot(6); 

XPd = Qdot(7); 

YPd = Qdot(8); 

% Coordinate Accelerations (rad/sec A 2 & m/scc A 2) 

ThOdd = Qddot(l); 

ThLldd =Qddot(2). 

ThL2dd = Qddot(3); 

ThRldd = Qddot(4); 

ThR2dd = Qddot(5); 

ThPdd = Qddot(6), 

XPdd = Qddot(7); 

YPdd = Qddot(8), 

% Angular Momentum 
HO = Th0d*(I0 + M0*Lc0 A 2); 

1 1L I = ThOd*(lL 1+ML1 *(LO A 2+LcLI A 2+2*LO*l.cL 1 *cos(ThL 1 ))) + 
ThL I d*(IL I+ML 1 *(LcL 1 A 2+L0*LcL 1 *cos(Thl . 1 ))); 

HL2 = ThOd*(IL2+ML2*(LO A 2+Ll A 2+LcL2 A 2+2*L()*Ll *cos(ThI,l) + 
2*LI*LcL2*cos(ThL2)+2*L0*LcL2*cos(ThLl+ThL2))) + ... 
ThLId*(lL2+ML2*(LI A 2+LcL2 A 2+L0*l l*cos(ThLl) + ... 

2*L 1 *LcL2*cos(ThL2)+LO*LcL2*cos(Thl . I+Thl.2))) + ... 
ThL2d*(lL2+ML2*(LcL2 A 2+Ll*LcL2*cos(ThL2) + ... 
L0*LcL2*cos(ThL l+ThL2))); 

1 1R 1 = ThOd*(IR 1+MRI *(R0 A 2+LcR I A 2+2*RO*LcR 1 *cos(ThR 1 ))) + . . 
ThR 1 d*(IR 1 +MR 1 *(LcR I A 2+R0*LcR I *cos(ThR 1 ))): 
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I1R2 = ThOd*(lR2+MR2*(RO A 2+R 1 A 2+LcR2 A 2+2*RO*R 1 *cos(ThR 1 ) + ... 
2*RI*LcR2*cos(ThR2)+2*R0*LcR2*cos(ThRI+ThR2))) + ... 
ThR 1 d*(l R2+MR2*(R 1 A 2+LcR2 A 2+RO*R 1 *eos(ThR 1 ) + ... 

2*R 1 *LcR2*cos(ThR2)+R0*LcR2*cos(ThR I +ThR2))) + ... 
ThR2d*(lR2+MR2*(LcR2 A 2+Rl*LcR2*eos(ThR2) + ... 
R0*LcR2*cos(ThR l+ThR2))); 

I IP = ThPd*lP + MP*(-XPd*YP + YPd*XP); 

1 ITotal = HO + HL 1 + HL2 + HR 1 + HR2 + HP; 

I Is = [HO HL1 HL2 HR1 HR2 HP I ITotal]; 



% Change in angular momentum 
HOd = Th0dd*(I0 + M0*Lc0 A 2); 

HL Id = ThOdd*(IL I +ML 1 *(L0 A 2+LcL 1 A 2+2*LO*LcLl *cos(ThL I))) + ... 

ThL I dd*(lL 1 +ML 1 *(LcL 1 A 2+L0*Lcl 1 *cos(ThL 1 ))) - ... 

ThOd*ThL ld*2*MLl *LO*LcL I *sin(Thl. 1 ) - ... 

ThL I d A 2*ML 1 *L0*LcL 1 *sin(ThL 1 ). 

1 IL2d = Th()dd*(lL2+ML2*(L0 A 2+L 1 A 2+LcL2 A 2+2*L0*L 1 *cos(ThL 1 ) + ... 
2*Ll*LcL2*cos(ThL2)+2*L0*LcL2*cos(ThLl+ThL2))) + ... 

ThL I dd*(IL2+ML2*(L 1 A 2+LcL2 A 2+L0*L 1 *cos(ThL 1 ) + ... 

2*L 1 *LcL2*cos(ThL2)+L0*LcL2*eos(ThL I +ThL2))) + ... 
ThL2dd*(IL2+ML2*(LcL2 A 2+LI*LcL2*cos(ThL2) + ... 

L0*LcL2*cos(ThL 1 +ThL2))) - ... 

ThOd*Thl,ld*2*ML2*(LO*Ll*sin(ThLn+LO*LcL2*sin(ThL l+'I'hl 2)) - ... 
ThOd*ThL2d*2*ML2*(LI*LcL2*sin(ThL2)+LO*LcL2*sin(ThLl+ThL2)) -... 
ThLld*ThL2d*2*ML2*(Ll*LcL2*sin(ThL2)+L0*LcL2*sin(ThLI+ThL2))-.. 
ThLld A 2*ML2*(LO*Ll*sin(ThLl)+LO*Lcl,2*sin(ThLl+ThL2)) - ... 
ThL2d A 2*ML2*(LI*LcL2*sin(ThL2)+LO*Lcl,2*sin(ThLl+ThL2)); 

HR I d = ThOdd*(lR I +MR I *(R0 A 2+LcR I A 2+2*RO*l ,cR 1 *cos(ThR 1 ))) + .. . 

ThR 1 dd*(lR 1 +MR 1 *(LcR 1 A 2+R0*LcR 1 *cos(ThR 1 ))) - ... 

Th()d*ThR 1 d*2*MR 1 *RO*LcR 1 *sin(ThR 1 ) - ... 

ThR ld A 2*MR I *R0*LcR 1 *sin(ThR 1 ): 

HR2d = ThOdd*(lR2+MR2*(RO A 2+R 1 A 2+LcR2 A 2+2*RO*R I *cos(ThR I ) + ... 

2*R 1 *LcR2*cos(ThR2)+2*R()*LcR2*cos(ThR 1 +ThR2))) + ... 

ThR 1 dd*(IR2+MR2*(R 1 A 2+LcR2 A 2+R( >* R I *cos(ThR 1) + ... 

2*R 1 *LcR2*eos(ThR2)+R0*LcR2*cos('ThR 1 +'HiR2))) + ... 
ThR2dd*(lR2+MR2*(LcR2 A 2+R 1 *LcR2*cos(ThR2) + ... 
R0*LcR2*cos(ThRl+TlrR2))) - ... 

ThOd*ThRld*2*MR2*(RO*RI*sin(ThI^l)+RO*LcR2*sin(ThRl+ThR2)) - ... 
Th0d*ThR2d*2*MR2*(Rl*LcR2*sin(ThR2)+R0*LcR2*sin(ThRI+ThR2))-. 
ThRId*ThR2d*2*MR2*(Rl *LcR2*sin(ThR2)+RO*LcR2*sin(ThRl+ThR2)) 
ThR ld A 2*MR2*(R()*R 1 *sin(ThR 1 )+RO*l,cR2*sin(ThR 1 +ThR2)) - ... 
ThR2d A 2*MR2*(Rl*LcR2*sin(ThR2)+RO*I.cR2*sin(ThRl+ThR2)); 

HPd = ThPdd*lP + MP*(-XPdd*YP - XPd*YPd + YlMd*XP + YPd*XPd); 

HdTotal = HOd + HL Id + HL2d + HR Id + HR2d + HPd: 

Hdols = [HOd HlHd HL2d HR Id HR2d HPd HdTotal]: 



C. Draw3 

% Filename is 'Draw3.m' 



function[X,Y] = Draw3(Lengths,AngConst,AngHist.Intenal) 




% This file draws the dual arm spacecraft stick figure 
% 

% INPUTS: 

% 

% Lengths = 7x1 vector of link lengths (m) 
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% 1st element is distanee from origin to left arm mount 

% 2nd & 3rd elements wrt left ami (from shoulder toward wrist) 

% 4th element is payload length 

% 5th & 6th elements wrt right aim (from wrist toward shoulder) 

% 7th clement is distance from right aim mount to origin 

% AngConst = vector of angles to ami mounting locations wrt ccntcrhody coord 
% frame (angle for left aim then angle for right aim) 

% AngHist = n\6 matrix of angle histories. Each row represents a 
% specific time. Each eolumn represents a specific joint 

% angle (except the payload angle is inertial) 

% 1 st column is the center body angle 

% 2nd & 3rd columns arc the left aim shoulder and elbow 

% 4th & 5th columns arc the right ami shoulder and elbow' 

% 6th column is the payload (this angle is inertial) 

% Interval = plot even' "intervarth" tunc 
% 

% 

% OUTPUTS; 



% 

% X = vector history of joint x coordinates 
% Y = vector history of joint y coordinates 

% X & Y treat the system as a closed chain beginning at the ccntcrbodv origin, 
% outward along the left arm, across the payload, inw ard along the right arm, 

% and back to the origin. 

% 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 



[Times,dummy] = size(AngHist); 

Links = length(Lengths); 

X(1 ,1) = 0; 

Y(U) = °, 

% Convert the joint angles to inertial angles and reorder them for closed chain use 
NAng(:,l) = AngHist(:,l) + AngConst(l)*ones(Times,l), 

NAng(:,2) = NAng(:,l) + Angl list(:,2); 

NAng(:,3) = NAng(:,2) + Angllist(:,3); 

NAng(:,4) = AngHist(:,6); 

NAng(:,7) = AngHist(:,l) + AngConst(2)*ones(Times,l ) + pi; 

NAng(:,6) = NAng(:,7) + AngHist(:,4); 

NAng(:,5) = NAng(:,6) + Angl list(:,5); 

p= 1; 

w hile p <= Times 
for q = 1 ; Links 
Lastx = 0; 

Lasty = 0; 
for r = l:q 

Lastx = Lastx + Lengths(r)*cos(NAng(p,r)), 

Lasty = Lasty + Lengths(r)*sin(NAmg(p,r)), 
end 

X(q+l,p) = Lastx; 

Y(q+l,p) = Lasty; 
end 

p = p + Interval, 
end 

X = [X(l :Links,:); X(2,:); X(Links,:); X(Links+l,:)]; 

Y = [Y(l :Links,:); Y(2,:); Y(Links,:); Y(Links+l,:)], 

% Plot the Final Case 
for q = L.Lmks 
Lastx = 0; 

Lasty = 0; 
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for r = l:q 

Lastx = Lastx + Lengths(r)*cos(NAng(Times,r)), 

Lasty = Lasty + Lengths(r)*sin(NAng(Times,r)); 
end 

XFinal(q+l , 1 ) = Lastx; 

YFinal(q+l,l) = Lasty; 
end 

XFinal = [XFinal(I :Links,:), XFinal(2,:); XFinal(Links,:). XFinal(Links+l,:)]; 
YFinal = [YFinal(I :Links,:); YFinal(2,:); YFinal(Links f :); YFinal(Links+l,:)]; 

clg; 

axis(’square') 

plot(X, Y,'-', XFinal. YFinal,’-', XFinal, YFinal, \\ X(:,l),Y(:,1Vo’); 

xlabel('X (m)');ylabel('Y (m)’); 

axis('normar) 



D. Eqn2 

% Filename is 'Eqn2.m' 

% Differential Equations for numerical integrator 
function [Xdot,U,TorqRef,Aqdot,J,Res,LHS,RHS,Delq] =... 

Eqn2(T,X,Ls,Ms,CMs,Is,BoundC,Gains,XfDes,Wu.We,Coef,ConstMat) 

% OUTPUT: 

% xdot = denvatives of state veetor at time T 

% U = eolumn veetor of aetual torques commanded at time T arranged 
% as [U 1 ; U2; U6, U5] where the number denotes the joint 
% associated with that torque 

% TorqRef = column vector of reference torques that should be applied 
% at time T if the motion followed the reference maneuver exactly. 

% These are arranged in the same order as U. 

% Res = column veetor of residuals after EOM are evaluated with the 
% calculated reference torques. (Residuals should be zero). 

% Aqdot = eolumn veetor of A*qdot. This is a test to see if the 
% constraint equation (A*qdot = 0) is satisfied. 

% LHS = eolumn veetor of the EOM left hand side (LI IS = M*qddot + GTilda) 
% RMS = eolumn veetor of the EOM right hand side (RI IS = BTilda*u) 

% 

% INPUTS: 

% T = time (see) 

% State Vector, X, is defined as follows: 

% XI = Theta 0 (rad) 

% X2 = Theta LI (rad) 

% X3 = Theta L2 (rad) 

% X4 = Theta R I (rad) 

% X5 = Theta R2 (rad) 

% X6 = Theta P (rad) 

% X7 = X component of Payload Center of mass position (m) 

% X8 = Y component of Payload Center of mass position (m) 

% X9 = Theta 0 Dot (rad/see) 

% X 1 0 = Theta L I Dot (rad/see) 

% XI 1 = Theta L2 Dot (rad/sec) 

% XI 2 = Theta Rl Dot (rad/scc) 

% X 1 3 = Theta R2 Dot (rad/see) 

% X 1 4 = Theta P Dot (rad/see) 

% X 1 5 = X component of Payload Center of mass velocity (m/sec) 

% XI 6 = Y component of Payload Center of mass velocity (m/see) 

% XI 7 = integral of the absolute value of the centerbody disturbance torque 
% X18 = integral of the centerbody disturbance torque squared 
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% Ls = 7x1 vector of lengths (m) 

% 1 st element = distance from origin to left arm mount 

% 2nd & 3rd elements wrt left arm (from shoulder toward wrist) 

% 4th element = payload length 

% 5th & 6th elements wrt right arm (from wrist toward shoulder) 

% 7th element = distance from right arm mount to origin 

% [L0;L1;L2;LP;R2;R1;R0] 

% Ms = 6x1 column vector containing the masses (kg) 

% 1st element = mass of spacecraft ccntcrbody 
% 2nd & 3rd elements = mass of left arm (upper arm then lower arm) 
% 4th & 5th elements = mass of right arm (upper arm then lower ami) 
% 6th element = payload mass 
% [MO; ML1; ML2; MR1; MR2; MPJ 

% CMs = 6x1 column vector containing center of mass locations 
% [LcO; LeLl; LcL2; LcRl ; LcR2; LcP] 

% Is = 6x1 column vector containing the moments of inertias about the 
% respective body's center of mass (kg m A 2) 

% 1st element = inertia of spacecraft centerbody 
% 2nd & 3rd elements = inertia of left arm (upper arm then lower arm) 
% 4th & 5th elements = inertia of right arm (upper arm then lower arm) 
% 6th element = payload inertia 
% [10; ILL, IL2; 1RL IR2; IP] 

% BoundC = boundry conditions for the problem. The first column 
% contains the initial x and y component of points Q & P 
% respectively, the x component of the right arm base, the 
% problem start time, and the simulation stop time. The second 

% column contains the x and y component of points Q & P 

% respectively, the x component of the right arm base, the 
% stop time for the ideal reference maneuver, and a Hag to 
% activate or deactivate the controller. The origin for the 
% x and y components is the base of the left arm. 

% Wu = 6x6 control torque cost weighting matrix 
% Wc = 8x8 constraint cost weighting matrix 
% Gains = 1x2 column vector of controller gains. The first value is 
% for position gains and the second value is for velocity 
% gains. 

% XfDes = column vector containing desired values for the angles at 
% the conclusion of the maneuver. These arc the same angles 

% the reference maneuver is trving to create. They are arranged 

% as [ThOf; ThLlf; ThL2f; ThRlf; ThR2f; ThPf].* 



%%%%%%%%%% 0 /o 0 /o 0 /o 0 /o% 0 /o 0 /o 0 /o% 0 /o 0 /o% 0 /o%%%%%%% 0 /o 

%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
%%%%%%%%%%%% 0 / 0 %%%%% 0 / 0 %%%%%%%%%% 0 / 0 % 
ThO =X(1); 

ThL 1 =X(2); 

ThL2 = X(3), 

ThR 1 = X(4); 

ThR2 = X(5); 

ThP = X(6); 

Xc = X(7); 

Yc = X(8); 

ThOd = X(9); 

ThL Id = X(10); 

ThL 2d = X(1 1); 

ThR Id = X(12), 

ThR2d = X(13); 

ThPd = X(14); 

Xed = X(1 5); 

Ycd = X( 1 6); 
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% Arms mount locations wrt spacecraft centerbody coordinate frame (rad) 
ThLO = BoundC(5, 1 ); ThRO = BoundC(5,2); 

% Stop Times 

TfR = BoundC(6,2); % Reference Torque stop time (sec) 

TfC = BoundC(7,l ); % Controller stop time (see) 

% Controller Flag 
ContFlag = BoundC(7,2); 

% Constraints Matrix Flag 
AMatFlag = BoundC(8, 1 ); 

% Centerbody Reaction Wheel Flag 
WheelFlag = BoundC(8,2); 

% Kinetic Energy Test Flag 
KEFlag = BoundC(llJ); 

% Inverse Kinematics Bypass Flag 
ByPass = BoundC(l 1,2); 

% Torque selection if bvpass is enabled 
TorqFlag = BoundC(12,l); 

% Maximum torque from reaction wheel 
TorqCap = BoundC(13,l); % Limit enabled 
TorqMax = BoundC(13,2); % Limit amount 

% Controller Gains: 

Gpos = Gains(l); 

Gvel = Gains(2); 



%%%%%%%%%%%%%% 

%% CALCULATIONS %% 

%%%%%%%%%%%%%% 

% ROM: M*qddot + dV/dq + G = Qf + A'*Lam 
% M is mass matrix 

% qddot is column vector of generalized coordinate accelerations 
% dV/dq is the partial derivative of the potential function with 
% respect to the generalized coordinates This term is zero for 
% this problem because all motion is in the horizontal plane (there 
% is no change in potential energy caused by the motion) 

% G is a column vector which is a function of q and qdot 
% Qf are generalized forces caused by joint torques 
% A' is transpose of constraints matrix 
% Lam are Lagrange multipliers 



%%%%%%% 0 /o 0 /o% 0 /o% 0 /o%%%%% 

%% State Vector & Derivative %% 
%%%%%%%%%%%%%%%%%% 

Q = [ThO; ThLl; ThL2; ThRl; ThR2; ThP; Xc; Yc]; 

Qdot = [ThOd, ThLld; ThL2d; ThRld; TliR2d; ThPd; Xcd; Ycd]; 



%%%%%%%%% 

%% Matrices %% 

%%%%%%%%% 

AngConst = [ThLO; ThRO]; 
if AMatFlag 

[M,G,A,Adot,B] = MatxFix(Ls,Ms,CMs,Is,Q,Qdot,AngConst); 

else 
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[M,G,A,Adot,B] = Matx(Ls,Ms,CMs,Is,Q,QdouAngConst); 

end 

if Wheel Flag 

B7 = [!;(); 0, 0, 0; 0; 0; 0]; 

B = [B7 BJ; 
end 

if ByPass % If true, then bypass calculating torques using inverse 
% kinematics. This branch of logic is a verification test 
% during program development and is not intended for regular 
% use once the program is cheeked out. 
if TorqFlag == 0 

U = zeros(6, 1 ); J = 0; 

else 

if TorqFlag = 1 

U = [-0.01; 0; 0; 0; 0; 0J; J = 0; 

else 

U = [-0.0 1; 0; 0; 0.01 ; 0; 0]; J=0; 
end 

end 

if WheelFlag 
U = [0; U]; 
end 

else % Normal program How to find control torques 
%%%%%%%%% 

%% Torques %% 

%%%%%%%%% 

if T <= TfR, % Get the appropriate torque and angle values 
% from the reference maneuver calculations 
[TorqRef, QRef, QdotRef, Aqdot, J, C 1 Ref, C2Ref, C3Ref] = ... 

RentLs^HCMsJs^oundC/UW^We^oe^ConstMat); 
else % Simulation is longer than ideal reference maneuver 
% Use no reference torques 
% Use the desired final values as references 
TorqRef = [0; 0; 0; 0; 0; 0]; 

QRef(l) = XfDes(l); 

QRef(2) = XfDes(2); 

QRef(3) = XfDes(3); 

QRef(4) = XfDes(4); 

QRef(5) =XlDes(5); 

QRef (6) = XfDes(6); 

QRef(7) = XfDes(7); 

QRef(8) =XlDes(8); 

QdotRef(l ) = XfDes(9); 

QdotRef(2) = XfDes(lO); 

QdotRef(3) = XfDes(l 1 ); 

QdotRef(4) = XfDes( 1 2); 

QdotRef(5) = XfDes(13); 

QdotRef(6) = XfDes( 1 4); 

QdotRef(7) = XfDes(15), 

QdotRcf(8) = XfDes(l 6); 
if WheelFlag 

TorqRef = [0; TorqRef); 
end 

% Matrices 
if AMatFlag 

[MRef,GRef,ARef,AdotRef] = MatxFix(Ls,Ms,CMsJs,.. 

QRef,QdotRef,AngConst); 

else 
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|MRef,GRef,ARef,AdotRef] = Mat\(Ls,Ms,CMs,Is,QRef,QdotRef,... 

An g Const), 

end 

BRef = B; 

Pt IRef = ARef*inv(ARef*inv(MRef)*ARef), 

CIRef = inv(MRef)*(eye(M) - PtlRef*ARef*inv(MRef))*BRef; 

C2Ref = -inv(MRef)*Pt 1 Ref*AdotRef; 

C3Ref = inv(MRef)*(PtlRef*ARef*mv(MRen - eve(M))*GRef; 



if ConlFlag % Controller is on 
Delq = Q - QRef ; 

Delqdot = Qdot - QdolRef ; 

% Controller calculations 
Ptl = A’*inv(A*inv(M)*A'); 

Cl = inv(M)*(eve(M) - Ptl*A*inv(M))*B, 

C2 = -inv(M)*Ptl *Adol; 

C3 = inv(M)*(Ptl *A*inv(M) - eye(M))*G; 

F2 = Gpos * Delq; 

F2 = [F2(l :6); 0, 0]; 

Kv = Gvel * eve(M); 

Kv(7,7)=0; Kv(8,8)=0; 

Pl3 = pinv(Cl); 

% Pt3 = inv(Cl'*Cl)*Cr; % Resulted in poorly conditioned matrix 



%%%% 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o%% 0 /o% 0 /o 0 /o%%%% 

%% Complete Lyapunov Controller %% 

%%%%%% 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o%%%% 0 /o 

U = Pl3*(-Kv*Delqdot + CIRePTorqRef - (C2*Qdot - C2Ref*QdotRer) - ... 
(C3 - C3Re0 - F2); 

%%%%%%%%% 0 /o% 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o%%%%% 

%% Simplified Lyapunov Controller %% 

%% (removes reference torques and %% 

%% assumes C2 and C3 terms are small) %% 
%%%%%%%%%%%%%%%%%%%%%% 

% Kp = Gpos * eye(M); 

% Pl3 = pinv(ClRef), 

% U = Pt3*(-(Kv+C2Re0*Delqdot - Kp^Delq) + TorqRef; 

else % Controller is off 

U = TorqRef; % Don't adjust torques from reference maneuver 
Delq = 999*ones(8.1 ); % Dummy value for trajectory error 
end % End of Control Loop 

if WbeelFlag 
J = abs(U( 1 )); 

else 

J = 0; 
end 

end % End of Kinetie Energy Test Conditional 

if TorqCap % Upper limit on wheel torque enabled? 
if abs(U( 1 )) > TorqMax 
if U(l)> 0 

U(l) = TorqMax; 

else 

U(l) = -TorqMax; 
end 

end 

end 



%%%%%%% 
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%% Qf %% 

%%%%%%% 

% Qf = B*u These are the generalized forces 
Qf=B * U; 



%%%%%%%%%%%%%%% 

%% Lagrange Multipliers %% 

%%%%%%%%%%%%%%% 

% EOM: M*qddot + dV/dq + G = Qf + A'*Lam 

% Solving the EOM for qddot gives: qddot = inv(M)*(Qf + A'*Lam - G) 
% Differentiating the Pfai'fian form of the constraint equations 
% results in: Adot*qdot + A*qddot = 0. 

% Substitution of the expression for qddot into the previous equation 
% permits solving for Lam: 

% Lam = inv(A*inv(M)*A’)*(A*inv(M)*(G-Qf) - Adot*qdot); 

Lam = inv(A*inv(M)*A’)*(A*inv(M)*(G-Qf) - Adot*Qdot); 



%%%%%%%%%%%%%%% 

%% Putting it all together %% 
%%%%%%%%%%%%%%% 

Qddot = inv(M) * (Qf + A’*Lam - G), 

[I Is, Hdots] = AngMo2(Ls,Ms,CMs,Is,Q,Qdot, Qddot); 
% Change in total angular momentum 
Hd = Hdots(7); 

J=[J; Hd], 



% Assemble derivative of state vector for integrator 
Xdot = [Qdot; Qddot; J(l); (J(1)) A 2]; 



%%%%%%%%%%%%%%% 
%% Troubleshooting Info %% 
%%%%%%%%%%%%%%% 
Aqdot = A*Qdot; 

LI IS = M*Qddot + G; 

RI IS = Qf + A’*Lam; 

Res = LI IS - RHS; 



E. fminu2 

function [\\OPTIONS] = fminu2(FUN,x,0PT10NS,GRADFUN,Pl,P2,P3,P4,P5,P6,... 
P7,P8,P9,P10) 

%FMINU Finds tlie minimum of a function of several variables. 

% X=FMINU( , FUN',X0) starts at the matrix XO and finds a minimum to the 
% function which is described in FUN (usually an M-file: FUN.M). 

% The function ’FUN’ should return a scalar function value: F=FUN(X). 

% 

% X=FMINU(’FUN , ,XO,OPTIONS) allows a vector of optional parameters to 
% be defined. OPTIONS(l) controls how much display output is given; set 
% to 1 for a tabular display of results, (default is no display: 0). 

% OPTIONS(2) is a measure of the precision required for the values of 
% X at the solution. OPTIONS(3) is a measure of the precision 
% required of the objective function at the solution 
% For more information type HELP FOPTIONS 
% 

% X=FMINU( , FUN , ,XO,OPTIONS, , GRADFUN’) enables a function’GRADFUN' 
% to be entered which returns the partial derivatives of the function, 

% df/dX, at the point X: gf = GRADFUN(X). 

% 
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% The default algorithm is the I3FGS Quasi-Newton method with a 
% mixed quadratic and cubic line search procedure. 

% Copyright (c) 1990 by the Math Works, Inc. 

% Andy Grace 7-9-90. 



% Initialization 

XOUT=x(:); 

nvars=length(XOUT); 

evalstr = [FUN]; 
if~any(FUN<48) 
evalstr=[evalstr, '(x r ]; 
for i=l margin - 4 

evalstr = [ evalstr, , ,P',num2str(i)]; 

end 

evalstr = jcvalstr, 
end 

if nargin < 3, OPTIONS=[]; end 
if nargin < 4, GRADFUN=[]; end 



if lcngth(GRADFUN) 
evalstr2 = [GRADFUN]; 
if ~any(GRADFUN<48) 
evalstr2 = [evalslr2, *(x'J; 
for i=l margin - 4 

evalstr2 = [evalstr2,',P , ,num2str(i)]; 

end 

evalstr2 = [evalstr2, *)']; 
end 
end 

f = eval(evalstr); 
n = length(XOUT); 

GRAD=zeros(nvars, 1 ); 

OLDX=XOUT; 

MATX=zeros(3,l); 

MATL=[f;0;0J, 

OLDF=f; 

FIRSTF=f; 

[OLDX,OLDF,I lESS,OPTIONS]=optint(XOUT,f,OPTIONS); 
CHG = Ie-7*abs(XOUT)+le-7*ones(nvars,l); 

SD = zeros(nvars, I ); 
diff = zeros(n vars, 1 ); 



OPTIONS(IO)=2; % Iteration count (add 1 for last evaluation) 
status =-l; 

while status ~= 1 
% Work Out Gradients 
if -length(GRADFUN) | 0I y T10NS(9) 

OLDF=f; 

% Finite difference perturbation levels 

% First check perturbation level is not less than search direction 
f ■ = find(10*abs(CHG)>abs(SD)); 

CHG(f) = -0. 1 *SD(f); 

% Ensure w ithin user-defined limits 
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CHG = sign(CHG+cps).*min(ma\(abs(CHG),0PT10NS(16)),0PT10NS(17)); 
for gcnt=l:nvars 

XOUT(gcnt, 1 )=XOUT(gcnt)+Cl lG(gcnl); 

OPTIONS( 1 0)=OPTlONS( 1 0)+ 1 ; 
dispC While Loop Iteration in Progress'); 
dispd'Iterations; ',num2str(Ol y riONS(10))]); 
dispd'Allowablc: \num2str(OPTIONS(14))J); 
x(:) = XOUT; f = cval(evalstr); 

GRAD(gcnt)=(f-OLDF)/(CHG(gcnt)); 
if f < OLDF 
OLDF=f; 

else 

XOUT(gcnt)=XOUT(gcnt)-CHG(gcnt); 

end 

end 

% Try to set difference to lc-8 for next iteration 
CHG = lc-8. /GRAD; 
f = OLDF; 

% OPTlONS(I0)=OPTIONS(10)+nvars; 

% Gradient check 

if OPTIONS(9)== 1 
GRADFD = GRAD; 
x(:)=XOUT; GRAD = eval(evalstr2); 
graderr(GRADFD, GRAD, evalstr2); 

0PT10NS(9) = 0; 

end 



else 

OPTIONS( 1 1 )=OPldONS( 1 1 )+l ; 
x(:)=XOUT; GRAD = eval(evalstr2); 



end 

% Initialization of Search Direction 

if status == -1 



SD=-GRAD; 

FlRSTF=f; 

OLDG=GRAD; 

GDOLD=GRAD'*SD; 

% For initial step-size guess assume the minimum is at zero. 

OPTIONS(18) = max(0.01, min([l,2*abs(f/GDOLD)])); 
if OPTlONS(l)>0 

% disp([sprintf( , %5.0f%12.3g%12.3g , ,Ol y riONS(10)X... 

OPTIONS( 1 8)),sprintf('% 1 2. 3g \GDOLD)]); 
end 

XOUT=XOUT+OPTIONS(l 8)*SD; 
status=4; 

if OPTIONS(7)==0; PCNT=1; end 
else 

% Direction Update 

gdnew=GRAD'*SD; 
if OPTIONS(1)>0, 

num=[sprintf('%5.0f % 1 2.3g % 1 2.3g ',OPTIONS( 1 ()),f,OPTIONS( 1 8)),... 
sprintf('%12.3g ’,gdne\v)]; 

end 

if (gdnew>0 & f>FlRSTF)hfinite(0 

% Case 1: New function is bigger than last and gradient w.r.t. SD -vc 
% ...interpolate, 
how-inter'; 

[stepsize]=cubici 1 (f,FlRSTF,gdnew,GDQLD,0PT10NS( 1 8)); 
if stepsize<0|isnan(stepsizc), stcpsize=OPTl()NS(18)/2; how-Clf'; end 
if 0PT10NS( 1 8)0. 1 &OPTIONS(6)==0 
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if stepsize*norm(SD)<eps 
rand(’normar) 
stcpsizc=rand(l); 

how- RANDOM STEPLENGT1 f; 
status=0; 

else 

stepsize=stepsize/2; 

end 

end 

OPTIONS( 1 8)=stepsize; 

XOUT=OLDX; 
clscif f<FIRSTF 

[newstep,fbest] =cubici3(f,FIRSTF,gdnew,GDOLD,OPTIONS(18)); 

sk=(XOUT-OLDX)’*(GRAD-OLDG); 

if sk>le-20 

% Case 2: New function less than old fun. and OK for updating HESS 
% .... update and calculate new direction 

how-’; 

if gdnewO 

how-incstep’; 
if newstep<OPTIONS( 1 8) 

newstep=2*OPTI ONS( 1 8)+ 1 e-5 , 
how=[how,' IF']; 

end 

OPTIONS( 1 8)=min([max([2, 1 .5*OPTIONS( 1 8)]), 1 +sk+abs(gdnew)+... 
max([0,OPTIONS(18)-I]), (1 .2-K).3*(~OPTIONS(7)))*ahs(newstep)]); 
else % gdnew>0 

if OPTIONS(18)>0.9 
how- int_sf; 

OPTIONS( 1 8)=min([ 1 ,abs(ncwstep)]); 

end 

end %if gdnew 

[HESS,SD]=updhess(XOUT,OLDX,GRAD,OLDG,flESS,OPlTONS), 

gdncw=GRAD'*SD, 

OLDX=XOUT; 

status=4; 

% Save Variables for next update 
FIRSTF=f; 

OLDG=GRAD; 

GDOLD=gdnew; 

% If mixed interpolation set PCNT 

if OPTlONS(7)==0, PCNT=I ; MATX=zeros(3, 1 ); MATL(l)=f; end 
clscif gdnew>() %sk<=0 

% Case 3: No good for updating HESSIAN .. interpolate or halve step length, 
how- interest'; 
if OPTIONS! 1 8)>0.01 

OPTIONS(I8)=0.9*newstep; 

XOUT=OLDX; 

end 

if OPTIONS! 18)>1, OPTIONS! 18)=1; end 

else 

% Increase step, replace starting point 

OPTIONS(18)=max([mm![newstep-OriTONS(18),3|),0.5*OI r nONS(I8)]); 

how-incst2'; 

OLDX=XOUT; 

FIRSTF=f; 

OLDG=GRAD; 

GDOLD=GRAD'*SD; 

OLDX=XOUT; 
end % if sk> 

% Case 4: New function bigger than old but gradient in on 
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% ...reduce step length, 
else %gdne\v<0 & F>FIRSTF 
if gdne\v<0&f>FIRSTF 
how- red_step'; 

if norm(GRAD-OLDG)<le-10; HESS=cyc(nvars); end 
if abs(0PT10NS(18))<eps 
randCnormaf) 

SD=norm(SD)*rand(SD) 

OPT10NS( 1 8)=abs(rand( 1 ))* 1 e-6; 
how-RANDOM SD'; 

else 

OPT1 ONS( 1 8)=-OPTl ONS( 1 8)/2 ; 
end 

XOUT=OLDX; 
end %gdncw>0 

end % if (gdnew>0 & F>FlRSTF)Hmite(F) 
XOUT=XOUT+OPTIONS( 1 8)*SD; 
if OPTIONS(1)>0 



% disp([num,hovv]) 

end 

end % End of Direction Update- 



% Check Termination 

if max(abs(SD))<2*0PT10NS(2) & (GRAD'*(SD)) < 2*0PT10NS(3) 
if OPTlONS(l) > 0 
disp(");disp(");disp("); 
disp(");disp(");disp( M ); 

disp('Optimization Tenninated Successfully'); 

% disp('Gradicnt less than options(2)'); 

disp([' NO OF ITERATIONS-, num2str(OPTlONS(10))]); 

end 

status=l ; 

el seif 0PT10NS( 1 0)>OPTlONS( 1 4) 
if OPTIONS( 1 )>=() 
disp(");disp(");disp("); 
disp(");disp(");disp("); 

disp('Waming: Maximum number of iterations has been exceeded'); 
disp(' - increase options(14) for more iterations ') 
end 

status=l ; 

else 

% Line search using mixed polynomial interpolation and extrapolation, 
if PCNT~=0 

while PC NT > 0 

OPTIONS( 1 0)=OPTIONS( 1 0)+l ; 
disp(");disp(");disp("); 
disp('Termination Check in Progress’); 
disp(['lterations: ',num2str(OI r riONS(10))j); 
x(:) = XOUT; 

f = eval(evalstr); 

[PCNT,MATL,MATX,steplen,f, how]=searchq(PCNT,f,OLDX,. .. 

MATL,MATX,SD,GDOLD,OPTIONS(18), how); 
0PT10NS(18)=steplen; 

XOUT=OLDX+steplen*SD; 

if abs(steplen) < le-6, PCNT=0; status=l; end 

end 



x(:)=XOUT; 

OPTIONS( 1 0)=OPTIONS( 1 0)+l ; 
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d isp( " ) ;d i sp( " ) ,d i sp(" ) ; 
disp('Terniination Check in Progress'); 
dispd'Itcrations: \num2str(OI y riONS(10))]), 
f = eval(cvalstr); 
end 

end 

end 

x(:)=XOUT; 

disp("),disp(");disp("); 

disp('');disp(''),disp( M ); 

disp(");disp( M );disp( M ); 

disp('Final Evaluation in Progress'); 

f = eval(evalstr); 

if f > FIRSTF 

OPT10NS(8) = FIRSTF; 

x(:)=OLDX; 

else 

OPTIONS(8) = f; 
end 



F. MainMin 

% Filename is "MainMin. m" 

% This is the routine used by "MainOpt.m" to optimize the reference 
% trajectory polynomial coefficients. It is a sealed down version 
% of the dual arm spacecraft program, "Main2.m". This version does 
% not integrate the state variables not include a Lyapunov controller. 

% The only integration that does take place is the optimization cost 
% function. 

function | JOpt] = MainMm(UpCoef,ConstMat,Flags) 

%clg;clear; 

format compacLformat short; 
k = lcngth(UpCoef); 

A543 = inv(ConstMat(:,k+l :k+3))*([l; 0; 0] - ConstMat(: J :k)*UpCoef); 
Coef = [UpCoef; A543J; % Reference trajectory polynomial coefficients 

% Reference Maneuver Start and Stop Times 
TO = 0; 

TfR = 10; 

TfC = 10; 

MctaFlag =Flags(l); 

ContFlag = Flags(2), 

PertFlag = Flags(3); 

AMatFlag = Flags(4); 

WheelFlag = Flags(5); 

EOMFlag = Flags(6), 

PlnvFlag =Flags(7); 

KL’Flag = Flags(8); 

OutFlag = Flags(9); 

Trace = Flags(lO); 

SymFlag =Flags(ll); 

Bypass =Flags(12); 

TorqFlag =Flags(13); 

Tol = le-6; % Integration tolerance 
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R2D = 180/pi, % Conversion factor from radians to degrees 



% Lengths (m) 

LO = 0.75, % Origin to left shoulder 

LI =0.5; % Left upper ami 

1,2 = 0.5; % Left forearm 

LP=0.5; % Payload 

R2 = 0.5; % Right forearm 

R 1 = 0.5; % Right upper arm 

R0 = sqrt(2*0.75 A 2); % Origin to right shoulder 

Ls = [L0; L 1 ; L2, 1,P, R2; R 1 ; R0]; 



% Member masses (kg) 

M0 = 5, 

ML1 = 1; 

ML2 = 1; 

MR1 = 1; 

MR2 = 1; 

MP = 1; 

Ms = [M0, ML 1 ; ML2; MR 1 ; MR2, MP], 

% Center of mass distances (m) 

LcO =0; 

LcLl =0.25; 

LcL2 = 0.25, 

LcRl =0.25; 

LcR2 = 0.25; 

LcP =0.25, 

CMs = [LcO; LcL 1 ; LcL2; LcRl ; LcR2; LcP]; 

% MOl about center of mass: Ic = (1/1 2)*(mass)*(length) A 2 
10 = M0, 

%10 = 0 ; 

11.1 = (1/12)* Ml, 1 * LI A 2; 

1L2 = (1/12) * ML2 * L2 A 2; 

IR1 = (1/12) * MR1 * R 1 A 2; 

1R2 = (1/12) * MR2 * R2 A 2; 

IP = (1/12) * MP * LP A 2; 
ls = [10; ILL, IL2; 1R1; IR2; IP], 

% Nominal initial and desired final locations of payload 
% Point Q is at wrist of left arm 
% Point P is at wrist of right arm 
Q.\0n = 0.125, Qy0n=1.5; 

Px0n = 0.625; Py0n=1.5; 

Qxf =0.125; Qyf = 1.0; 

Pxf =0.125, Pyf = 1.5, 

% Nominal initial and desired final locations of centerbody 
ThOO = 0; 

ThOf = 0/R2D, 



% Arms mount locations wrt centerbody coordinate frame (rad) 
ThLO = pi/2; 

ThRO = pi/4; 

AngConstf 1 ) = ThLO; 

AngConst(2) = ThRO; 

% Symmetric geometry to center arms and test kinetic energy 
if SymFlag 
ThLO = 3*pi/4; 
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AngConst(l) = ThLO; 

RO = LO, % Origin to right shoulder 
Ls(7,1) = RO; 

QxOn = -0.25; QyOn =1.2; 

PxOn = 0.25, PyOn = 1.2; 
end 



BoundC (1,1) = QxOn; 
BoundC(2,l) = PxOn; 
BoundC(3, 1 ) = Qxf; 
BoundC(4,l) = Pxf; 
BoundC(5, 1 ) = ThLO; 
BoundC(6,l ) = TO; 
BoundC(7,l) = TfC; 
BoundC(8,l) = AMatFlag; 
BoundC(9,l) = ThOO; 
BoundC(10,l)= EOMFlag; 
BoundC(l 1,1)= KEFlag; 
BoundC(12,l)= TorqFlag; 



BoundC (1,2) = QyOn; 
BoundC(2,2) = PyOn; 
BoundC(3,2) = Qyf. 
BoundC(4,2) = Pyf; 
BoundC(5,2) = ThRO; 
BoundC(6,2) = TfR; 
BoundC(7,2) = ContFlag. 
BoundC(8,2) = WheelFlag 
BoundC(9,2) = ThOf; 
BoundC(10,2)= PlnvFlag: 
BoundC(l 1 ,2)= Bypass; 



% Weighting Matrices 

% Control torques are calculated to minimize the following cost function: 
% J = 0.5*(u'*Wu*u + (A'*Lamy*Wc*(A'*Lam)) 
if WheelFlag 

Wu = eye(7); % Control Torque Weighting 

else 

Wu = eye(6); 
end 

%if WheelFlag 
% Wu = zeros(7,7); 

%else 

% Wu = zeros(6,6), 

%end 

%Wu(4,4)=le5; 

%Wu(7,7)=le5; 

%Wu(2,2)=lel0, 

%Wu(5,5)=1e10; 

We = zeros(8,8); % Constraint Force Weighting 
%We = eye(8); 



%%%%%%%%%%%%%%%%% 

%% INITIAL CONDITIONS %% 

%%% 0 /o%%% 0 /o 0 /o%%%%% 0 /o%% 

% Desired Initial Payload Parameters 
ThPO = atan2(Py0n-Qy0n,Px0n-Qx0n); 
XcO = 0.5 ♦(PxOn + QxOn); 

YeO = 0.5 * (PyOn + QyOn); 

QxO = QxOn; 

QyO = QyOn; 

PxO = PxOn; 

PyO = PyOn; 



% Initial State 
XO = 0; 



%%%%%%%%%%%%% 

%% INTEGRATION %% 

%%%%%%%%%%%%% 

% RefMin2 uses change in angular momentum to find wheel command torque 
[T,Jlnt,J] = odemin(T<efMin2\T0,TfR,X0,Tol,TraeeXs,Ms,CMs,Is3oundC,... 
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Wu,We,Coef,ConstMat); 

% Optimization cost function is integral of J 
k = length(T); 

JOpt = Jlnt(k); 

%JOpt = max(abs(J)); 



G. MainOpt 

% Filename is "MainOpt.m" 

% This routine optimizes the dual arm spacecraft eost function 
% by changing the polynomial coefficients which describe the 
% reference trajectory. It calls "Main2.m" 

%clear 

ele 

home 

format compact 
format short 

IJpCoefO = [()]; % Starting Guess 

%UpCoefO = UpCoef; % Use last values for stalling guess 

k = length(UpCoefO); 

options = []; % Default values 

options(l) = 0; % Display during optimization eycle: l=On, 0=Off 

options(14) = 100*k; % Maximum number of iterations 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%% Flags during optimization %% 

%%%%%%%%%%% 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o% 0 /o 0 /o%%% 0 /o% 0 /o%% 
MetaFlag = 0; % Creates metafile named "main. met" 

ContFlag = 0, % Controller Status Flag: l=On; 0=0 IT 

PertFlag = 0; % Perturbation Flag (0=no perturbation, 1 =perturbation) 

% The perturbation is to check the controller by 
% disturbing the actual initial state away from nominal. 

% The reference torques are based on nominal. 

AMatFlag = 0; % Size of A matrix: 0 = 4x8 (Free Centerbody) 

% 1 = 5x8 (Fixed Centerbody) 

WheelFlag = 1 ; % Centerbody Reaction Wheel ( l=On, 0=()ff) 

EOMFlag = 8; % Specifies number of cost function constraint equations 
% 3 = only payload equations 

% 5 = only spacecraft equations 

% any other value = all 8 equations 

PlnvFlag = 1 ; % Psuedo-Inverse Flag (for use in finding reference torques) 

% 1 = Use psuedo-in verse 

% 0 = Use inverse 

KEFlag = 0; % KE Test Flag 

% 1 = Nonzero velocity initial conditions 

% 0 = Zero velocity initial conditions 

OutFlag = 0; % Output Flag 

% 1 = Display output 

% 0 = Don't display output 

Trace = 1; % Observe integration 

% 1 = Observe 

% 0 = Don't observe 

OptFlag = 0; % Optimization Flag 

% 1 = Perform optimization 

% 0 = Don't perform optimization 

SymFlag = 0; % Symmetric Geometry Flag 

% 1 = Symmetric geometry 
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% 0 = Nonsymmetrie geometry 

ByPass = 0; % Torque ealeulation bypass flag 

% 1 = Bypass 

% 0 = Use inverse kinematics 

TorqFlag= 0; % Torques to use if bypass enabled 

% 0 = No Torques (Drift) 

% 1 = One Shoulder Torque 

% 2 = Symmetric Shoulder Torques 

TorqCap = 0; % Maximum limit on wheel torque 

% 1 = Enabled 

% 0 = Disabled 

TorqMax = 0.075;% Limit on wheel torque if TorqCap enabled 
Flags 1(1) = MetaFlag; 

Flags 1 (2) = ContFlag; 

Flags 1(3) = PertFlag; 

Flagsl (4) = AMalFlag, 

Flagsl (5) = WheelFlag; 

Flagsl(6) = EOMFlag; 

Flagsl(7) = PInvFlag; 

Flagsl(8) = KEFlag; 

Flagsl(9) = OutFlag; 

Flagsl (10)= Trace; 

Flagsl(l 1)= SymFlag; 

Flags 1(1 2)= ByPass; 

Flags 1(13)= TorqFlag; 

Flagsl (14)= TorqCap; 

Flagsl(15)= TorqMax; 

%%%%%%%%%%%%%%%% 

%% Flags after optimization %% 
%%%%%%%%%%%%%%%% 



Flags2 = Flagsl; 
Flags2(l) = 0; 
Flags2(2) = 1; 
Flags2(3) = 0; 
Flags2(5) = 1; 
Flags2(8) = 0; 
Flags2(9) = 1 ; 
Flags2(10)= 1; 
Flags2(l 1 )= 0; 
Flags2(12)= 0; 
Flags2(13)= 0; 

Flags2(14)= 0; 
Flags2( 15)= 0.075 
Flags2(16)= 0; 

DiaFlag = 0; 



ConstMat = ones(3 
for n=l:k+3 



% MetaFlag: l=On, 0=Off (File is "main. met”) 

% Controller Flag: l=On, 0=OfT 

% Perturbation Flag: l=On, 0=Off 

% Wheel Flag: l=On, 0=Off 
% Kinetie Energy Flag: l=On, 0=011 
% Output Flag: l=On, 0=Off 
% Traee Flag: l=On, 0=OlT 
% Symmetric Geometiy Flag: l=Sym, 0=NonSym 
% Inverse Kinematies Bypass: l=Bypass, 0= Inverse Kinematics 
% Torq Flag. 0=No Torq, l=One Torq, 2=T\vo Symmetric Torqs 
% Torq Flag is for when the bypass is enabled 
% TorqCap: l=On,()=Off 
% Limit on maximum wheel torque 
% DocFlag: l=On,0=Off 
% separate meta files for eaeli page ("doc#. met") 

% Diary Flag 

% 1 = Create diary file "main dia" 

% 0 = No diary file 

M3); 



ConstMat(2,n) = k-H5-n; 

ConstMat(3,n) = ConstMat(2,n)*(ConstMat(2,n)-l); 
end 

if OptFlag 

[UpCoefoptions] = fminu2('MainMin , ,UpCoef0,options,[],ConstMat, Flagsl ); 
end 



if DiaFlag 
diary main dia 
end 
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if -OptFlag 
UpCocf = UpCocfO; 
end 

[Jlnt] = Main2(UpCocf,ConstMat,Flags2); 

% Plot position, velocity, & acceleration reference trajectories 
k = lcngth(UpCoef); 

A 54 3 = inv(ConslMat(:,k+l :k+3))*([ 1; 0; 0] - ConstMat(:,l:k)*UpCoef), 
Coef = [UpCoef ; A543J, % Reference trajectory polynomial coefficients 

k = length(Coef); 

Steps = 21; 
for rn = 1 .Steps 
Tau = (m- 1 )/( Steps- 1 ); 
for n=l :k 

CTau(k+l-n) = Coef(k+l-n)*Tau A (n+2); 

CTaud(k+l-n) = Coef(k+l-n)*Tau A (n+l ); 

CTaudd(k+l-n) = Coef(k+l-n)*Tau A (n); 
end 

W(m) = ConstMat(l,:)*CTau'; 

Wd(m) = ConstMat(2,:)*CTaud'; 

Wdd(m) = ConstMat(3,:)*CTaudd', 
end 
elg 

T=0:1 /(Steps- 1): 1 ; 
subplot(221) 

plot(T,W);title('Position vs Normalized Time'); 
xlabel('Tau (sec)');ylabel('Position'); 
subplot(222) 

title('Refcrencc Trajectories') 
subplot(223) 

plot(T,Wd);title('Velocity vs Normalized Time'); 
xlabel('Tau (sec)');ylabei(' Velocity'); 
subplot(224) 

plot(T,Wdd);titlc('Accelcration vs Normalized Time'); 
xlabel('Tau (sec)'),ylabel('Acceleration'); 
if Flags2(l) 
meta main 
end 

if Flags2(16) 
meta doe6 
end 
pause 

disp('lnilial guess for highest order eoefficients');disp(IJpCoefO'), 
disp('Cocfficients in descending order');disp(CoeO; 
disp('lntegral of Cost Function,(JlntAbs & JlnlSqr)');disp(Jlnt), 
if OptFlag 

di.sp('lterations');disp(options(10)); 

end 

diar> r off 



H. Main2 

% Filename is "Main2.m" 

% This routine is the driver for the dual arm spacecraft problem 
% but is called by "MainOpt.m" after the polynomial reference trajectory 
% coefficients have been optimized. 
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function [JlntTotal] = Main2(UpCocf,ConstMat,Flags) 

% Calculate the coefficients for orders five, four, and three. 

% Include these with the higher order coefficients in a vector, 
k = lcngth(lJpCoef); 

A543 = inv(ConstMat(:,k+l:k+3))*([l; 0; 0] - ConstMat(:,l :k)*UpCocf); 
Coef = [UpCoef; A543]; % Reference trajectory polynomial coefficients 



%%%%%%%% 

%% Times %% 

%%%%%%%% 

% Reference Maneuver Start and Stop Times and Controller Stop Times 
% Setting the controller time longer than the reference maneuver time 
% ensures that the controller eliminates any errors remaining after the 
% reference trajectory should be complete. To exereise the controller 
% only with no reference trajectory, set the reference maneuver stop 
% time to a negative value. 

TO = 0; 

TfR = 10; 

TfC = 10; 

MetaFlag =Flags(l); 

ContFlag = Flags(2); 

PertFlag = Flags(3); 

AMatFlag = Flags(4); 

WheelFlag = Flags(5); 

FOMFlag = Flags(6); 

PlnvFlag =Flags(7); 

KEFlag = Flags(8); 

OutFlag = Flags(9); 

Trace = Flags(lO); 

SymFlag = Flags(ll); 

By Pass = Flags(12); 

TorqFlag =Flags(13); 

TorqCap = Flags( 1 4); 

TorqMax = Flags( 1 5); 

DoeFlag = Flags( 1 6); 

Pert = -10; % Perturbation of initial payload angle, ThctaP (deg) 

Tol = le-6; % Integration tolcranee 

Interval = 3, % Stick figure drawing includes every lnterval'di time 
R2D = 180/pi; % Conversion factor from radians to degrees 



%%%%%%%%%%%%%% 

%% System Parameters %% 
%%%%%%%%%%%%%% 

% Lengths (m) 

L0 = 0.75; % Origin to left shoulder 

L 1 = 0.5; % Left upper arm 

L2 = 0.5; % Left forearm 

LP = 0.5; % Payload 

R2 = 0.5; % Right forearm 

R1 = 0.5; % Right upper arm 

R0 = sqrt(2*0.75 A 2); % Origin to right shoulder 

Ls = fLO; LI; L2; LP; R2; Rl; R0]; 

% Member masses (kg) 

M0 =5; 

ML1 = 1; 

ML2 = 1; 

MR1 = 1; 
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MR2 = I; 

MR = 1 , 

Ms = [MO; ML I, ML 2, MR I; MR2; MP]; 

% Center of mass distances (m) 

LcO =0; 

Lei. I =0.25; 

LcL2=0.25; 

LcRl =0.25; 

LcR2 = 0.25; 

LcP =0.25; 

CMs= [LcO; Lei. 1 ; Lcl.2; LcRl; LcR2; LcP]; 

% MOI about individual centers of mass 

% Arms arc modelled as slender rods: lc = ( I/I 2)*(mass)*(lcngth) A 2 
It) = M0; 

11.1 = (1/12) * ML1 * L 1 A 2; 

11.2 = (1/12) *ML2*L2 A 2; 

1RI = (1/12) * MR1 *R1 A 2, 

IR2 = (1/12) * MR2 * R2 A 2; 

IP = (1/1 2) * MP *LP A 2; 

Is =[10; IL1; 1L2; IR1; IR2; IP], 

% Nominal initial and desired final locations of payload 
% Point Q is at wrist of left arm 
% Point P is at wrist of right arm 
Q\0n = 0.125; QyOn = 1.5; 

Px0n = 0.625; Py0n=1.5; 

Qxf =0.125, Qyf =1 .0; 

Pxf =0.125; Pyf = 1.5; 

% Nominal initial and desired final locations of centcrbody 
ThOO = 0/R2D; 

ThOf = 0/R2D; 

% Arms mount locations wrt centcrbody coordinate frame (rad) 

Thl.O = pi/2; 

ThRO = pi/4; 

AngConst(l) = ThLO; 

AngConst(2) = ThRO; 

% Symmetric geometry' to center amis and test kinetic energy 
if SymFlag 
ThLO = 3*pi/4; 

AngConst(l) = ThLO; 

R0 = L0; % Origin to right shoulder 
I.s(7,I) = R0; 

QxOn = -0.25; QyOn = 1.2; 

PxOn = 0.25; PyOn = 1.2; 
end 



% Assemble information required in other subroutines into a matrix 



BoundC(l,l) = QxOn; 
BoundC(2,l) = PxOn; 
BoundC(3,l) = Qxf; 
BoundC(4,l) = Pxf, 
BoundC(5,l) = ThLO; 
BoundC(6,l) = TO; 
BoundC(7,l ) = TfC; 
BoundC(8, 1 ) = AMatFlag; 
BoundC(9, 1 ) = ThOO; 



BoundC( 1 ,2) = QyOn; 
BoundC(2,2) = PvOn; 
BoundC(3,2) = Qyf; 
BoundC(4,2) = Pyf; 
BoundC(5,2) = ThRO; 
BoundC(6,2) = TfR. 
BoundC(7,2) = ContFlag; 
BoundC(8,2) = WhcclFlag; 
BoundC(9,2) = ThOf; 
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BoundC(10,l)= EOMFlag; P3oundC(10,2)= PlnvFlag; 

1 3oundC( 11,1)= KEFlag, BoundC( 11,2)= By Pass; 

BoundC(12,l)= TorqFlag; 

BoundC(13,l)= TorqCap; BoundC(I3,2)= TorqMax; 

% Gip are gains for angle i position error 
% Giv are gains for angle i velocity error 
Gpos = 0.5; % Position error gain 

Gvel = 0.2; % Velocity error gain 

Gains = [Gpos; Gvel], 

% Weighting Matrices 

% Control torques are calculated to minimize the following cost function 
% J = 0.5*(u'*Wu*u + (A , *Lam)'*We*(A'*Lam)) 

% Wu is the control torque weighting matrix 
% We is the constraint force weighting matrix 
if WheelFlag 

Wu = eye(7); % Control Torque Weighting 

else 

Wu = eye(6); 
end 

%if WheelFlag 
% Wu = zeros(7,7); 

%else 

% Wu = zeros(6,6); 

%end 

%Wu(4,4)=le5; % Penalty on wrist motors for free centerbody ease 

%Wu(7,7)=le5; 

%Wu(2,2)=lel0; % Penalty on wrist motors for fixed centerbodv ease 

%Wu(5,5)=lelO, 

We = zeros(8,8); % Constraint Force Weighting 

%We = eye(8); 

%%%%%%%%%%%%%%%%% 

%% INITIAL CONDITIONS %% 

%%%%%Vo%%%%%%%%%%% 

% Desired Initial Payload Parameters 
ThPO = atan2(Py0n-Qy0n,Px0n-Qx0n); 

XcO = 0.5 * (PxOn + QxOn); 

YeO = 0.5 * (PyOn + QyOn); 

if PertFlag % Perturbation enabled 

ThPO = ThPO + Pert/R2D; % Perturb payload angle 
QxO = XeO - LeP*eos(ThP0); % Perturb arm end points 
QyO = YeO - LeP*sin(ThPO); 

PxO = XeO + (LP-LeP)*eos(ThP0); 

PyO = YeO + (LP-LeP) ,( 'sin(TliPO); 
else % No Perturbation 

QxO = QxOn, 

QyO = QyOn; 

PxO = PxOn; 

PyO = PyOn; 
end 

PertCrd = [QxO QyO PxO P> ()]; 

% Left Arm 

% Elbow is left of line from arm base to Q (RQ) 

LSx = LO * eos(ThOO + ThLO); 

LSy = LO * sin(ThOO + ThLO); 

RQ = sqrt((Qx()-LSx) A 2+(Q)^0-LSy) A 2); % Length from arm base to Q 

Betal = atan2(QyO-LSy,QxO-LSx); % Angle from arm base to RQ 
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% Law of cosines: cos(A) = (b A 2 + c A 2 - a A 2)/(2be) 

% Appiv to find angle between RQ and Link LI 
Num = Ll A 2 + RQ A 2-L2 A 2; 

Den = 2 * L 1 * RQ, 

Bcta2 = acos(Num/Den), % Angle from RQ to Link 1 

ThL 10 = (Betal + Beta2) - (ThOO + ThLO); % Theta LI 
% IJsc law of cosines to find the interior angle at the elbow 
Nuin = L 1 A 2 + L2 A 2 - RQ A 2; 

Den = 2 * L 1 * L2; 

Bcta3 = acos(Num/Den); 

ThL 20 = -(pi-Bcta3), 

% Right Ami 

% Elbow is right of line from arm base (shoulder) to P (wrist) (RP) 

RSx = R0 * cos(Th00 + ThRO); 

RSy = R0 * sin (ThOO + ThRO); 

RP = sqrt((PxO-RSx) A 2+(PyO-RSy) A 2); % Length from arm base to P 
Betal = atan2(Py0-RSy,Px0-RSx); % Angle from arm base to RP 
% Law of cosines: cos(A) = (b A 2 + c A 2 - a A 2)/(2bc) 

% Apply to find angle between RP and Link R1 
Num = R1 A 2 + RP A 2 - R2 A 2; 

Den = 2 * R1 * RP; 

Bcta2 = acos(Num/Dcn); % Angle from Link R1 to RP 

Bcta4 = Betal - (ThOO + ThRO); 

ThR 10 = -(Bcta2 - Beta4), 

Num = R1 A 2 + R2 A 2 - RP A 2; 

Den = 2 * R 1 *R2; 

Bcta3 = acos(Num/Den); 

ThR 20 = pi - Bcta3; 

% Desired Initial State 

X0 = [ThOO; ThL 10; ThL20; ThR 10; ThR20; ThPO, XcO; YeO;... 

0; 0; 0; 0; 0; 0, 0; 0]; 

%%%%%%%%%%% 0 /o 0 /o 0 /o 0 /o%%%%% 

%% Kinetic Energy- Test Conditions %% 
%%%%%%%%%%%%%%%%%%%% 

% Specify Payload and Centerbody Initial Rates 
% Compatible Rates for the Redundant Coordinates arc calculated 
if KEFlag 

ThPdO = 0/R2D; % Rates to specify 
XedO = -0.03; 

YedO = -0.03; 

ThOdO = 0/I^2D, 

%%%%%%%%%%% 

%% LEFT ARM %% 

%%%%%%%%%%% 

% [Qxd; Qyd] - [Hl]*Th0d + [H2]*Thd 

% Qxd & Qyd are x & y components of point Q inertial velocity. 

% Thd = [ThL 1 dot; ThL2dot] 

% 1 1 matrices are made from expressing the x & y components of Q in 
% terms of L0, ThO, ThLO, LI, ThLl, L2, and ThL2. 

% Qx=L0*cos(Th0+ThL0)+L 1 *cos(ThO+ThLO+TL 1 )+L2*cos(Th0+. 
% ThL0+ThLl+ThL2) 

% Qy=L0*sin(Th0+ThL0)+L 1 *sin(Th0+ThL0+ThL 1 )+L2*sin(Th0+. 

% ThLO+TliL l+ThL2) 

% The differentiation of these equations lead to 
% [Qxd; Qyd] = [II 1 ]*Th0d + [H2]*Thd which can be solved for Thd 
QxdO = XcdO + LcP * ThPdO * sin(ThPO); 

QvdO = YedO - LcP * ThPdO * cos(ThPO); 

112(1,2) = -L2*sin(ThOO+ThLO+ThL 1 0+ThL20), 
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112(1,1)= 112(1,2) - 1.1 *sin(ThOO+ThLO+Thl. 1 0), 

1 12(2,2) = L2*eos(Th00+ThL0+ThL10+ThL20); 

1 12(2,1) = 112(2,2) + LI*cos(ThOO+Thl.O+Thl. 10): 

111(1,1)= H2( 1,1)- L0*sin(Th00+ThL0), 

111(2,1) = 112(2, 1 ) + L0*eos(Th00+ThL0); 

ThdO = inv(H2) * ([QxdO; QydOj - HI *ThOdO); 

% Angle rates 
ThLldO = ThdO(l); 

ThL2dO = ThdO(2); 

%%%%%%%%%%%% 

%% RIGHT ARM %% 

%%%%%%%%%%%% 

% The development is similar to the left arm 
% Px=RO*cos(ThO+ThRO)+Rl *eos(ThO+ThRO+ThR I )+R2*cos(Th()+... 
% ThRO+ThRI +ThR2) 

% Py=RO*sin(ThO+ThRO)+R 1 *sin(ThO+ThRO+ThR I )+R2*sin(ThO+... 
% ThRO+ThRI +ThR2) 

% [ Pxd; Pyd] = [Hl]*ThOd + [H2]*Thd 
PxdO = XcdO - (LP - LeP) * ThPdO * sin(ThPO); 

PvdO = YcdO + (LP - LeP) * ThPdO * cos(ThPO): 

1 12(1,2) = -R2*sin(Th00+ThR0+ThR10+ThR20); 

112(1,1)= H2( 1 ,2) - R 1 *sin(ThOO+ThRO+ThR 10): 

112(2,2) = R2*eos(Th00+ThR0+ThRI0+ThR20); 

1 12(2, 1 ) = 112(2,2) + R 1 *eos(ThOO+ThRO+ThR 1 0): 

111(1,1) = H2( 1,1) - RO*sin(ThOO+ThRO); 

111(2,1)= 1 12(2,1 ) + RO*cos(ThOO+ThRO): 

ThdO = inv(H2) * ([PxdO; PydO] - HI*ThOdO): 

% Angle rates 
ThRldO = ThdO(l): 

ThR2dO = ThdO(2); 

X0= [ThOO; ThLlO; ThL20; ThRIO; ThR20; ThPO: XcO; YcO;... 
ThOdO; ThLldO; TliL2dO; ThRldO; ThR2dO; ThPdO; XedO, YcdOl; 

end 



%%%%%%%%%%%%%%%% 

%% FINAL CONDITIONS %% 

%%%%%%%%%%%%%%%% 

% Desired Final Payload Angle 
ThPf = atan2(Pyf-Qyf,Pxf-Qxf); 

% Left Arm 

% IZlbow is left of line from arm base to Q (RQ) 

LSx = LO * eos(ThOf + ThLO); 

LSv = LO * sin(ThOf + ThLO); 

RQ = sqrt((Qxf-LSx) A 2+(Qyf-LSy) A 2); % Length from arm base to Q 
Beta I = atan2(Qyf-LSy,Qxf-LSx); % Angle from arm base to RQ 

% Law of eosines: eos(A) = (b A 2 + c A 2 - a A 2)/(2bc) 

% Apply to find angle between RQ and Link L 1 
Num = L 1 A 2 + RQ A 2 - L2 A 2; 

Den = 2 * L 1 * RQ; 

Beta2 = acos(Num/Den); % Angle from RQ to Link 1 

Till . 1 f = (Beta 1 + Beta2) - (ThOf + ThLO); % Theta 1 . 1 
% Use law of cosines to find the interior angle at the elbow 
Num = L1 A 2 +L2 A 2 - RQ A 2; 

Den = 2 *L1 * L2; 

Beta3 = aeos(Num/Den); 

ThL2f = -(pi-Beta3); 

% Right Ann 

% Clbow is right of line from ann base (shoulder) to P (wrist) (RP) 

RSx = RO * eos(ThOf + ThRO); 
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RSy = RO * sin(ThOf + ThRO); 

RP = sqrt((Pxf-RSx) A 2+(Pyf-RSy) A 2); % Length from arm base to I’ 
Betal = atan2(Pyf-RSy,Pxf-RSx); % Angle from arm base to RP 
% Law of cosines: eos(A) = (b A 2 + e A 2 - a A 2)/(2bc) 

% Apply to find angle between RP and Link R1 
Num = R I A 2 + RP A 2 - R2 A 2; 

Den = 2 * R I * RP; 

Beta2 = aeos(Num/Den); % Angle from Link R 1 to RP 

Beta4 = Betal - (ThOf + ThRO); 

ThRIf = -(Beta2 - Beta4); 

Num = R1 A 2 + R2 A 2 - RP A 2; 

Den = 2 * R I * R2; 

Beta3 = aeos(Num/Den); 

ThR2f = pi - Beta3; 

% Desired Final State 
Xef = 0.5 * (Pxf + Qxf); 

Ycf = 0.5 * (Pyf + Qyf); 

QfDes = [ThOf; ThLIf; ThL2f; ThRIf; ThR2f; TliPf; Xef; Ycf;... 

0, 0; 0; 0; 0; 0; 0; 0]; 



if OutFlag 

%%%%%%%%%%%%%%%%% 

%% PROBLEM SUMMARY %% 

%%%%%%%%%%%%%%%%% 
dispCPROBLEM SUMMARY’) 

disp(") 

disp(’lnitial Angles (deg)') 
disp('Initial Angular Rates (deg/see)') 
disp('Desired Final Angles (deg)') 

disp(' ThetaO ThetaLI ThetaL2 ThetaRI ThetaR2 ThetaP’) 

disp(X0(1 :6)'*R2D) 

disp(X0(9: 14)'*R2D) 

disp(QlDes(1:6)'*R2D) 

disp( M ) 

disp('Payload Coordinates (m)') 

dispC Nominal Initial, Perturbed Initial, and Final') 

disp(' Qx Qy Px Py ') 

TableCrd = [QxOn QyOn PxOn PyOn; PertCrd: Qxf Qyf Pxf Pyf), 

disp(TableCrd) 

disp( M ) 

disp(’Aim Mounting Locations vvrt Centerbody Coordinate Frame (deg)') 

disp(ThL0*R2D);disp(ThR0*R2D) 

disp(") 

disp('Start, Referenee Manuever Stop, & Simulation Stop Times (sec)’) 

disp(T0);disp(TfR);disp(TfC) 

dispO 

disp(’Controller Status (I = On; 0 = Of0') 

disp(ContFlag) 

disp( H ) 

disp('Perturbation Status (1 = On; 0 = Off)') 

disp(PertFlag) 

dispf) 

disp(’Centerbody Status in Forward EOM (1 = Fixed; 0 = Free)’) 

disp(AMatFlag) 

dispO 

dispCReaetion Wheel Status (I = On; 0 = Off)') 

disp(WheelFlag) 

dispf) 

disp('Number of Equations in Cost Function Constraint (3, 5 or 8)') 
disp(EOMFIag) 
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dispf) 

disp('Psuedo-ln verse Status (1 = On; 0 = Off)') 

disp(PInvFIag) 

dispf) 

disp('Nonzero Initial Velocity Status (I = On; 0 = Off)') 

disp(KEFIag) 

dispf) 

disp('Geometrv Status (I = Symmetric; 0 = Nonsymmetric)') 

disp( SymFlag) 

dispf) 

disp('Inverse Kinematics Bypass Status (I = Bypass. 0 = Use inv. kinematics)') 

disp(ByPass) 

dispf) 

disp(’Torques if Bypass Enabled (0=None, I=Onc, 2=Two Symmetric)') 

disp(TorqFIag) 

dispf) 

disp('Rcaction Wheel Torque Cap Status (I=EnabIcd, (HDisabled)') 

disp(TorqCap) 

if TorqCap 

disp('Limit on Wheel Torque'); 
disp(TorqMax); 
end 
dispf) 

disp(’Controllcr Gains (position and velocity)') 

disp(' Gpos Gvel') 

disp(Gains') 

dispf) 

disp('Cost Function: J = 0.5*(uT*Wu*u + (AT*Lam)T*Wc*(AT*Lam))') 

disp(' where T signifies transpose') 

disp('Control Torques Weighting Matrix, Wu') 
disp(Wu) 

%disp('Constraint Forces Weighting Matnx, Wc') 

%disp(Wc) 

end % End of OutFlag branch 

%%%%%%%%%%%%% 

%% INTEGRATION %% 

%%%%%%%%%%%%% 

% "ode" is a variable step size Runge-Kutta integrator function 
% supplied with MATLAB. "ode2" is the same as "ode" in its function 
% but permits the passing of more variables into and out of the function. 
[T,X,Torq,TorqRef,Aqdot,J,Res,LHS,RHS,DeIq] = ... 

odc2('Eqn2',T0,TfC,X0,Tol,Trace,Ls,Ms,CMsJs.BoundC,... 
Gains,QfDes,Wu,Wc,Coef,ConstMat); 
k = length(T); 

JInt = X(:,I7:18); 

JIntTotal = X(k, 17:1 8); 

if OutFlag 

%%%%%%%%%% 

%% OUTPUT %% 

%%%%%%%%%% 



clg; 

% Angle Histories 
n = length(T); 

Q = X(:,I:6); 
subplot(221) 

pIot(T,Q(:,I)*R2D,T,Q(:,2)*R2D,T,Q(:,3)*R2D,... 

T>Q(:,4)*R2D,T,Q(:,5)*R2D,T,Q(:,6)*R2D); 
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hold on 

plot(T(n),Qmcs(I)*R2D;*\T(nXQfDcs(2)*R2D:*\T(n),Qroes(3)^R2D:*\... 

T(n),QfDes(4)*R2D,'*7r(n),QfDes(5)*R2D,'*Yr(n),QfDes(6)*R2D,'*'); 
litlefThetas vs Time’); 

\label('Timc (see)');ylabel('Angles (deg)'); 
hold off 

% Angle Rate Histories 
Qdot = X(:,9: 14), 
subpk>t(223) 

plot(T,Qdot(:,l)*R2D,T,Qdot(:,2)*R2D,T,Qdot(:,3)*R21\.. 

T,Qdot(:,4)*R2D,T,Qdot(: > 5)*R2D,T,Qdot(:,6)*R20); 
title('ThetaDots vs Time’); 

\label(Time (see)');ylabel(’Angle Rates (deg/see)'); 

%Departures from Referenee Trajectory 
if -ByPass 

subplot(222) 

plot(T,Delq(l,:)*R2D,T,Delq(2,:)*R2D,T,Delqf3,:)*R2D,... 

T,pelq(4,:)^R2DJ,Delq(5,:)*R2DJ > Delq(6,:)*R2D); 
tille('Displaeement Errors vs Time'); 
xlabelfTime (sec)’);ylabcl('Q-QRef (deg)'); 

end 

if MetaFlag 
meta main 
end 

if DocFlag 
meta doe 1 
end 
pause 

% Draw Motion 
Angles = Q(:,l :6); 

[Xeoord,Ycoord] = Draw3(Ls, AngConst, Angles, Interval); 
if MetaFlag 
meta main 
end 

if DoeFlag 
meta doe2 
end 
pause 

dispf); 

dispCSTATE VECTOR TIME HISTORY:'); 
disp(’Angles (deg)’) 

Table 1 = [T X(:,1:6)*R2D]; 

disp(' Time ThetaO ThetaLl ThetaL2 ThetaRI ThctaR2 ThetaP'); 

disp(Tablel) 

pause 

disp("); 

disp(' Angle Rates (deg/sec)') 

Table2 = [T Qdot(:,l:6)*R2DJ; 

disp(' Time ThOdot ThLldot ThL2dot ThRldol ThR2dot ThPdot'); 

disp(Table2) 

pause 



if -ByPass 
dispC); 

dispCTRAJECTORY ERROR TIME HISTORY:'): 
disp('Angles (deg)') 
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Table2a = [T R2D*Delq(l :6,:)T, 

dispC Time DelThO DelThLl DelThL2 DelThRl DelThR2 DelThF); 
disp(Table2a) 



end 

pause 

% Reference Torque Histories 
elg; 

if TfR > 0 
if TfR < TfC 

|r,s] = size(TorqRef); 

TorqRef = [TorqRef zeros(s,l)]; 

TRef = [T(l :r); TfR]; 
else 

TRef = T; 

end 

subplot(221) 
plot(TRef,TorqRef); 
title(’Referenee Torques vs Time'); 
xlabel(Time (seey);ylabel ('Reference Torques'), 
end 

% Command Torque Histories 
%Torq = [Torq, zeros(4,l)]; 
k=n; 

subplot(223) 
plot(T(l :k)',Torq); 
title('Command Torques vs Time’); 
xlabel(’Time (sec)');y label (’Command Torques’); 

% Cost Function 
subplot (222) 

plot(T,J(l ,:));title('Cost vs Time'); 
xlabel('Time (see)');y label (*J=abs(Uwh)'); 
subplot(224) 

plo((T,JInt);title('Integrated Cost vs Time'); 
xlabel(’Time (see)');ylabelCJInt'); 
if MetaFlag 
meta main 
end 

if DoeFlag 
meta doe3 
end 
pause 



if TfR > 0 
disp(") 

disp('REFERENCE TORQUE HISTORY'), 
if WheelFlag 

disp(' Tune UO ULS ULE ULW URS IJRE URW’), 
else 

dispC Tune ULS ULE ULW URS URE 
end 

Table4 = [TRef TorqRef ]; 
disp(Table4) 
end 
pause 
disp(") 

disp('COMMAND TORQUE HISTORY’), 
if WheelFlag 

dispC Time UO ULS ULE ULW URS URE URW’); 
else 



URW'); 
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dispO Time ULS ULE ULW l IRS URE URW’); 
end 

Table5 = fT(l :k) Torq']; 

disp(Table5) 

pause 

Tablc6 = [T(l :k) J(1 Jlnt]; 

disp("); 

dispCCOST FUNCTION HISTORY’); 
dispC Time J JlntAbs JlntSqr’); 
disp(Tab!e6); 
pause 

% Angular Momentum 
k = length(T); 
for n = 1 :k 

[I Is] = AngMo(Ls,Ms,CMs,Is,X(n,l :8),X(n,9:16)); 
if n = 1 

HHist = Hs; 

else 

HHist = (HHist; Hs]; 
end 
end 
clg 

subplot(221); 

plot(T,HHist(:J :6));title('Angular Momentum of Pieees vs Time'). 
xlabel(’Time (see)’);ylabel(’Ang Momentum (N-m-sec)'), 
subplot(223); 

plot(T,HHist(:,7));titlc('Total Angular Momentum vs Time'); 
xlabel('Time (sec)’);ylabel('Ang Momentum (N-m-see)’); 

% Kinetic Energy 
for m = 1 :k 
if AMatFlag 

[M,G>A,Adot,B] = MatxFix(Ls,Ms,CMs,Is,X(m,l :8) f X(m,9:16),AngConst); 

else 

[M,G,A,Adot,B] = Malx(Ls,Ms,CMs,Is,X(m J :8),X(m,9:16),AngConst); 

end 

LHSTot(m) = 0; 

R1 ISTot(m) = 0; 

ResTot(m) = 0, 
for n=l ;8 

LHSTot(m) = LHSTot(m) + LHS(mm); 

RHSTot(m) = RHSTot(m) + RHS(n,m); 
end 

ResTot(m) = LHSTot(m) - RI ISTot(m); 

KE(m) = 0.5*X(m,9:16)*M*X(m,9:16)’; 
end 

subplot(224) 

plot(T,KE);title(’Kinetic Energy vs Time'); 
xlabel(’Time (sec)');ylabel('KE (kg m A 2/s A 2)'); 

% Compare wheel torque to change in total angular momentum 

I Id = J(2,:)’; 

subplol(222) 

plot(T( 1 :k)\Torq( I ,:),T( 1 :k)\Hd); 

title(’Compare Wheel Torque to Change in Ang. Mom.'); 

xlabel(’Time (sec)’); 

pause 

if MetaFlag 
meta main 
end 

if DocFlag 
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meta doc4 
end 

% pa use 



clg 

subplot(221) 

plot(T,Rcs);titlc('Residuals of Equations'); 
xlabcl(Time (sec)’);ylabel('LHS-RHS'); 
subplot(223) 

plot(T,ResTot);title('Total Residuals'); 
xlabeI('Time (see)');ylabeI(’LHS-RHS'); 

% Constraints: see if A*Qdot = 0 is satisfied 
subplot(222) 

plot(T(l :k),Aqdot(l /.),T(1 :k),Aqdot(2,:),T(l :k).Aqdot(3, :),... 

T( 1 :k),Aqdot(4,:)); 

[duml,dum2] = size(Aqdot); 
if duml ==5 
hold on 

plot(T(l:k),Aqdot(5,:)); 
hold off 
end 

titleCConstraints: A.*Qdot vs Time'); 
xlabcl('Timc (sec)’);ylabcl('A*Qdof); 
if MctaFlag 
meta main 
end 

if DoeFlag 
meta doc5 
end 
pause 

end % End of OutFlag braneh 



I. Matx 

% Filename is 'Matx.m' 

% This routine calculates the matrices for the dual ami 
% spacecraft EOM when it is grasping a payload Each aim 
% has two links. This version assumes that the ccnlerbody 
% is NOT fixed. This impacts A and Adot. 

function [M,G,A,Adot,B] = Matx(Ls,Ms,CMs,Is,Ths,Thdots,AngConst) 

% OUTPUTS: 

% M = 8x8 mass matrix 

% G = 8x1 vector w ith eoriolis and centripetal terms 
% A = 4x8 constraints matrix 
% Adot = 4x8 derivative of constraints matrix 
% B = Control influence matrix 

% 

% INPUTS: 

% Ls = 7x 1 vector of lengths (m) 

% 1st element = distance from origin to left arm mount 
% 2nd & 3rd elements wrt left ami (from shoulder to wrist) 

% 4th element = payload length 

% 5th & 6th elements wrt right arm (from w rist to shoulder) 

% 7th element = distance from right aim mount to origin 

% [LO; LI; L2; LP; R2; Rl; RO] 

% Ms = 6x1 eolumn veetor containing the masses (kg) 
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% 1 st element = mass of spacceraft centerbodv 

% 2nd & 3rd elements = left arm (upper then lower arm) 

% 4th & 5tli elements = right arm (upper then lower arm) 

% 6th element = payload mass 
% | MO; ML 1 , ML 2; MR 1 ; MR2; MP] 

% CMs = 6x 1 column vector containing center of mass locations (m) 
% [LcO; LeLl; LcL2; LcRl; LeR2, LeP] 

% Is = 6x 1 column vector containing the moments of inertias 
% about the respective body’s center of mass (kg m A 2) 

% 1 st element = inertia of spacecraft centcrbody 

% 2nd & 3rd elements = left arm (upper then lower arm) 

% 4th & 5th elements = right arm (upper then lower arm) 

% 6th element = payload inertia 
% [10; 1L1; 1L2; 1R1; IR2; IP] 

% Ths = 6 element vector containing the angles which describe 
% the configuration of the system. 

% [ThO; ThL 1 ; ThL2; TliRl; ThR2; ThP) 

% Thdots = 6 element vector containing the angle rates 
% AngConst = 2 element vector of arm mounting locations 
% [ThLO; ThRO] 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 

%%%%%%%%%%%%%%% 0 /o 0 /o 0 /o 0 /o% 0 /o%%%%%%%%%% 

% Lengths (m) 

L0 = Ls(l); 

LI = Ls(2); 

L2 = Ls(3); 

LP = Ls(4); 

R2 = Ls(5); 

RI = Ls(6); 

RO = Ls(7); 

% Member masses (kg) 

MO =Ms(l); 

ML1 =Ms(2); 

ML2 = Ms(3); 

MR1 = Ms(4), 

MR2 = Ms(5), 

MP =Ms(6); 

% Center of mass distances (m) 

LcO = CMs(l); 

LcL 1 =CMs(2); 

LeL2 = CMs(3); 

LcRl = CMs(4); 

LeR2 = CMs(5); 

LeP = CMs(6); %measurcd from left end 

% MOl about center of mass 
10 = ls( 1 ); 

I L 1 =Is(2); 

1L2 = Is(3); 

IRI = Is(4); 

1R2 = Is(5); 

IP = Is(6); 

% Angles 
ThO = Ths(I); 

ThLl = Ths(2); 

ThL 2 = Ths(3); 
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ThRl = Ths(4); 

ThR2 = Ths(5); 

ThP = Ths(6); 

% Angle Rates 
ThOd = Thdots(l); 

Thl , Id = Thdots(2); 
ThL2d = Thdots(3); 
ThRld = Thdots(4); 
ThR2d = Thdots(5); 
ThPd = Thdots(6); 

% Arm mount locations 
TliLO = AngConst(l); 
ThRO = AngConst(2); 



%%%%%%%%%%% 

%% Mass Matrix %% 

%%%%%%%%%%% 

M = zeros(8,8); 

M(8,8) = MP; 

M(7,7) = MP, 

M(6,6) = IP; 

M(5,5) = IR2 + MR2*LcR2 A 2; 

M(5,4) = M(5,5) + MR2*R 1 *LcR2*cos(ThR2), 

M(4,5) = M(5,4); 

M(5, 1 ) = M(4,5) + MR2*RO*LcR2*cos(ThR 1 +ThR2); 

M(l,5) = M(5,l); 

M(4,4)= M(4,5)+IRl+MR2*Rl*LcR2*cos(ThR2)+MRI *PeRl A 2+MR2*R1 A 2, 
M(4, 1 )=M(4 ,4 )+R0* (M R 1 *LcR 1 +MR2*R 1 )*cos(ThRl )+MR2*R()*l ,cR2*.. . 

cos(ThR 1 +ThR2); 

M(l ,4) = Mt4,l); 

M(3,3) = IL2 + ML2*LcL2 A 2, 

M(3,2) = M(3,3) + ML2*L i *LcL2*cos(ThL2)'. 

M(2,3) = M(3,2); 

M(3,l) = M(3,2) + ML2*L0*LcL2*cos(ThL I+Thl.2); 

M(l,3) = M(3,l); 

M(2,2) = M(3,2)+ML2*L 1 *LcL2*cos(ThL 2)+lI , 1 +M1 , 1 *LcL, 1 A 2+ML2*I, 1 A 2, 
M(2,l)=M(2,2)+L0*(MLl*LcLl+ML2*Ll)*cos(ThLl)+ML2*L0*LcL2*... 

cos(ThLl+ThL2); 

M( 1 ,2) = M(2,l); 

Parti = 10+M(2,2)+M(4,4)+MO*LcO A 2+(MlJ+M1.2)*I.() A 2+(MRl+MR2)*RO A 2; 
Pail2 = 2*L0*(ML 1 *LcL 1 +M1.2+L 1 )*cos(ThL 1 )+2*Ml,2*LO*l,el ,2*... 
cos(ThL 1 +ThL2); 

Part3 = 2*R0*(MR1 *LcRl+MIi2*Rl)*cos(ThRl)+2*MR2*RO*LcR2*... 

cos(ThRl+ThR2); 

M(l,l) = Parti +Part2 + Part3; 



%%%%%% 

%% C> %% 

%%%%%% 

G = zcros(8, 1 ); 

Pt 1 = -L0*(ThL 1 d A 2+2*Th0d*ThL 1 d)*(ML 1 *LcL 1 +ML2*I, 1 )*sin(ThL 1 ); 
Pt2 = -ML2*Ll*LcL2*ThL2d*(2*ThOd+2*ThI.ld+ThL2d)*sin(Thl,2); 
Pt3=-ML2*LO*LcL2*(2*ThOd*(ThLld+ThL2d)+(ThLld+ThL2d) A 2)*... 
sin(TliI,l+TliL2); 

Pt4 = -RO*(ThR 1 d A 2+2*Th()d*ThR 1 d)*(MR 1 *LcR 1 +MR2*R I )*sin(ThR 1 ); 
Pt5 = -MR2*Rl*LcR2*ThR2d*(2*Th0d+2*ThRld+ThR2d)*sin(ThR2); 
Pt6=-MR2*RO*LcR2*(2*ThOd*(ThRld+ThR2d)+(ThRld+ThR2d) A 2)*... 
sin(ThR 1 +ThR2); 

G(l) = Ptl + Pt2 + Pt3 + Pt4 + Pt5 + Pt6; 
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Pt I = LO*ThOd A 2*(MLl *I.cl, I +Ml,2*I.l )*sin(ThI. 1 ). 

Pt2 = -ML2*I J*LeL2*ThL2d*(2*ThOd+2* ThMd+TIiL2d)*sin(TIiL2); 
Pt3 = ML2*I,0*I.cl,2*Th0d A 2*sin(ThL]+Thl,2); 

G(2)=PtI + Pt2 + Pt3; 

(i(3)=ML2*LcL2*(lJ*(TIiOd+TIiI.ld) A 2*siii(Tlil.2)+I.()*TiiUd A 2* 

sin(ThbJ+Thl,2)); 

Pi I = R()*ThOd A 2*(MR 1 *LcR 1 +MR2*R I )*sin(ThR 1 ); 

Pt2 = -MR2*Rl*LcR2*ThR2d*(2*Th()d+2*rhRld+TI)R2d)*sin('HiR2). 
Pt3 = MR2*R()*l.cR2*rhOd A 2*sinrrhRH-riiR2); 

G(4) = Ptl + Pl2 + Pt3; 

0(5) = MR2*l.cR2*(Rl*(ThOd+TliRld) A 2*siiin'hR2)+R()*Th()d A 2* 
sin(ThRl+ThR2)); 



%%%%%%%%%%%%%% 

%% Constraints Matrix %% 

%%%%%%%%%%%%%% 

% The constraint matrix comes from putting the constraint equations 
% into the Pfaffian fonn: A*qdot + AO = 0. The first two constraint 
% equations arc found by finding the x and y components of the 
% Payload's center of mass by starting at the origin and moving 
% up the left arm. The second two constraint equations find the x 
% and y components of the Payload's center of mass bv starting at the 
% origin and moving to the base of the right aim and then 
% up the right arm. Differentiating these equations results 
% in the Pfaffian form with AO = 0. 

A = zeros(4,8); 

AT 1,7)= - 1 ; 

A(2,8) = - I ; 

A(3,7) = - I; 

A(4,8) = - I ; 

A(l,6) = -LeP*sin(ThP); 

A(2,6) = LcP*cos(ThP); 

A(3,6)= (I,P-LeP)*siii(ThP); 

A(4,6) = -(LP-LcP)*cos(ThP); 

A(4,5) = R2*cos(Th()+ThR0+fhR 1 +ThR2); 

A(4,4) = A(4,5) + R 1 *cos(Th()+ThR()+ThR I ); 

A(4,l) = A(4,4) + R()*cos(ThO+ThRO). 

A(3,5) = -R2*sin(Th0+ThR0+ThRl+ThR2), 

A(3,4) = A(3,5) - R1 *sin(Th()+ThR()+ThRl), 

A(3,l) = A(3,4) - R0*sin(Th()+ThR0), 

A(2,3) = L2*cos(ThO+ThLO+ThLl+ThL2); 

A(2,2) = A(2,3) + L 1 *cos(Th()+Thl,0+T hL 1 ); 

A(2,l) = A(2,2) + LO*cos(ThO+ThLO); 

A( 1 ,3) = -L2*sin(T h()+ThL()+ThL 1 +ThL2); 

AT 1,2) = ATI ,3) - Ll*sin(ThO+ThL()+ThLl); 

A( 1 ,1 ) = A(1 ,2) - LO*sin(ThO+'fhLO); 

Adot = zeros(4,8), 

Adot(l,6) = -ThPd*LcP*cos(ThP); 

Adot(2,6) = -ThPd*LcP*sin(ThP), 

Adot(3,6) = ThPd*(LP-LcP)*a)s(ThP), 

Adot(4,6) = ThPd *(LP-I.eP)*sin(ThP); 

Adot(4,5) = -(ThOd+ThRId+ThR2d)*R2*sin(Th()+ThR()+ThRl+ThR2); 
Adot(4,4) = Adot(4,5) - (ThOd+ThRld)»Rl*sin(ThO+ThRO+ThR 1); 
Adot(4,l) = Adot(4,4) - Th()d*RO*sin(ThO+ThRO); 

Adot(3,5) = -(ThOd+ThR 1 d+ThR2d)*R2*eos(ThO+ThRO+ThR I +ThR2 
Adot(3,4) = Adot(3,5) - (ThOd+ThR 1 d)*R 1 *cos(Th<)+ThRO+ThR 1 ); 
Adot(3,l ) = Adot(3,4) - ThOd*RO*cos(Th()+ThRO), 

Adot(2,3) = -(ThOd+ThLld+ThL2d)*L2*sin(ThO+Thl.()+ThLl+Thl,2), 
Adot(2,2) = Adot(2,3) - (ThOd+ThL 1 d)*L I *sin(ThO+ThLO+'l hL 1 ), 
Adot(2,l) = Adot(2,2) - Th0d*1.0*sin(Th0+ThL0), 
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Adot( 1 ,3) = -(ThOd+ThL 1 d+ThL2d)*L2*cos(Th()+Thl ,0+ThL I +ThL2); 
Adot( 1 ,2) = Adot( 1,3)- (ThOd+ThL 1 d)*L 1 *cos(ThO+TliL()+ThL 1 ); 
Adot(Ul) = Adot(I,2) - ThOd*LO*cos(Th(HThLO); 



%%%%%% 
%% B %% 
%%%%%% 

B = zero s( 8,6); 
B(l,3) = -1; 
B(l,6) = -1; 
B(2,I) = 1; 
B(2,3) = - I ; 
B(3,2) = 1; 
B(3,3) = -1; 
B(4,4) = 1; 
B(4,6) = -1; 
B(5,5) = 1; 
B(5,6) = -1; 
B(6,3)= l; 
B(6,6) = 1; 



J. MatxFix 

% Filename is 'MatxFix. m’ 

% This routine calculates the matrices for the dual arm 
% spacecraft EOM when it is grasping a payload Each aim 
% has two links. This version assumes that the ccntcrbody 
% is fixed. This impacts A and Adot. 

function [M,G,A,Adot,B] = Matx(Ls > Ms,CMs,Is,Ths,Thdots.AngConst) 

% OUTPUTS: 

% M = 8x8 mass matrix 

% G = 8x1 vector with coriolis and centripetal terms 
% A = 5x8 constraints matrix 
% Adot = 5x8 derivative of constraints matrix 
% B = Control influence matrix 

% 

% INPUTS: 

% Ls = 7x I vector of lengths (m) 

% I st element = distance from origin to left arm mount 
% 2nd & 3rd elements wrt left arm (from shoulder to wrist) 

% 4th element = payload length 

% 5th & 6th elements wrt right arm (from wrist to shoulder) 

% 7th element = distance from right arm mount to origin 

% [LO; LI; L2; LP; R2; Rl; RO] 

% Ms = 6x1 column vector containing the masses (kg) 

% I st element = mass of spacecraft centcrbody 
% 2nd & 3rd elements = left arm (upper then lower arm) 

% 4th & 5th elements = right arm (upper then lower arm) 

% 6th element = pavload mass 
% [MO, ML 1 ; ML2; MR 1 ; MR2; MP] 

% CMs = 6x1 column vector containing center of mass locations (m) 

% [LcO; LcL I ; LcL2; LcRl ; LcR2; LcP] 

% Is = 6x1 column vector containing the moments of inertias 
% about the respective body's center of mass (kg m A 2) 

% 1 st element = inertia of spacecraft centcrbody 

% 2nd & 3rd elements = left arm (upper then lower ami) 

% 4th & 5th elements = right arm (upper then lower arm) 
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% 6th element = payload inertia 
% [10; IL 1 ; IL2; IR I ; IR2, IP) 

% Ths = 6 element vector containing the angles w hich describe 
% the configuration of the system 
% [ThO; ThL I ; ThL2. ThR I ; ThR2; ThP) 

% Thdots = 6 element vector containing the angle rates 
% AngConst = 2 element vector of arm mounting locations 
% [Thl-0; ThROJ 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Lengths (m) 

1.0 = Ls(I); 

LI = Ls(2); 

L2 = Ls(3); 

LP = L,s(4); 

R2 = Ls(5); 

R 1 = Ls(6); 

R0 = Ls(7); 

% Member masses (kg) 

MO = Ms( 1 ); 

ML I = Ms(2); 

ML2 = Ms(3); 

MR I = Ms(4); 

MR2 = Ms(5); 

MP = Ms(6); 

% Center of mass distances (m) 

LeO =CMs(l); 

LcLl = CMs(2); 

LcL2 = CMs(3), 

LeRl = CMs(4); 

LeR2 = CMs(5); 

LeP = CMs(6); %measured from left end 

% MOI about eenter of mass 
10 = Is(I); 

1 L I = Is(2); 

1L2 = ls(3); 

IRI = Is(4); 

IR2 = Is(5); 

IP = Is(6); 

% Angles 
ThO = Ths( I ); 

ThL I = Ths(2); 

ThL 2 = Ths(3); 

ThR I = Ths(4); 

ThR2 = Ths(5); 

ThP = Ths(6); 

% Angle Rates 
ThOd = Thdots(l); 

ThL Id = Thdots(2); 

ThL 2d = Thdots(3); 

ThR Id = Thdots(4), 

ThR2d = Thdots(5); 

ThPd = Thdots(6), 
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% Ami mount locations 
ThLO = AngConst(l); 
ThRO = AngConst(2); 



%%%%%%%%%%% 

%% Mass Matnx %% 

%%%%%%%%%%% 

M = zcros( 8,8); 

M(8,8)=MP; 

M(7,7) = MP; 

M(6,6) = IP; 

M(5,5) = IR2 + MR2*I.cR2 A 2, 

M(5,4) = M(5,5) + MR2*R 1 *LcR2*cos(ThR2 ); 

M(4,5) = M(5,4); 

M(5, 1 ) = M(4,5) + MR2*R0*LcR2*cos(ThR 1 +ThR2); 

M(l,5) = M(5,l); 

M(4,4) = M(4,5)+IR I +MR2*R 1 *LcR2*cos(ThR2)+MR 1 *IxRl A 2+MR2*R 1 A 2; 
M(4, 1 )=M(4,4)+R0*(MR 1 *LcR 1 +MR2*R 1 )*cos(ThRl )+MR2*R0*LcR2*. .. 

cos(ThR 1 +ThR2); 

M(1,4) = M(4,1); 

M(3,3) = IL2 + ML2*LcL2 A 2; 

M(3,2) = M(3,3) + ML2*L 1 *LcL2*cos(ThL2); 

M(2,3) = M(3,2); 

M(3,l) = M(3,2) + ML2*L0*LcL2*cos(ThI, l+ThL2); 

M(1,3) = M(3,I); 

M(2,2) = M(3,2)+ML2*L 1 *LcL2*cos(TliL2)+lL 1+ML 1 *LcL 1 A 2+ML2*U A 2; 
M(2,l )=M(2,2)+L0*(ML 1 *LcL 1+ML2*L 1 )*cos(ThL 1 )+ML2*LO*LcL2*... 

cos(lhLl+ThL2); 

M(1,2) = M(2,1); 

Parti =IO+M(2,2)+M(4,4)+MO*LcO A 2+(MLl+ML2)*LO A 2+(MRl+MR2)*RO A 2; 
Part2 = 2*L0*(MLl*IxLl+ML2*Ll)*cos(ThLl)+2*MI,2*L0*Lcl,2*... 
cos(TliL 1 +ThL2); 

Part3 = 2*R0*(MR1 *LcRl+MR2*Rl)*cos(ThRl)+2*MR2*R0*LcR2*... 

cos(ThR 1 +ThR2); 

M(l,l) = Parti + Part2 + Part3; 



%%%%%% 

%% G %% 

%%%%%% 

G = zeros(8,l); 

Pt 1 = -L0*(ThI.ld A 2+2*Th0d*ThLld)*(ML l*LcL 1+ML.2*L l)*sin(ThL I); 
Pt2 = -ML2*LI*LcL2*ThL2d*(2*Th0d+2*Thlld+ThL2d)*sin(Thl,2); 
Pt3=-ML2*LO*LcL2*(2*Th()d*(ThI.ld+TliI.2d)+(ThLld+ThL2d) A 2)*... 
sin(ThLl+ThL2); 

Pt4 = -R()*(ThR 1 d A 2+2*ThOd*TliR 1 d)*(MR 1 *1 ,cR 1 +MR2*R 1 )*sin(ThR 1 ); 
Pt5 = -MR2*Rl*LcR2*ThR2d*(2*Th0d+2*ThRId+ThR2d)*sin(ThR2); 
Pt6=-MR2*R0*LcR2*(2*Th0d*(ThR I d+ThR2d)+(ThR 1 d+ThR2d) A 2)*. 
sin(ThRl+ThR2); 

G( 1 ) = Pt 1 + Pt2 + Pt3 + Pt4 + Pt5 + Pt6; 

Ptl = L0*Th0d A 2*(ML 1 *LcL 1 +ML2*L I )*sin(ThL 1 ); 

Pt2 = -ML2*Ll*LcL2*ThL2d*(2*Th0d+2*ThLld+ThL2d)*sin(ThL2); 

Pt3 = ML2*L0*LcL2*Th0d A 2*sin(ThL 1 +ThL2); 

G(2) = Ptl + Pt2 + Pt3; 

G(3) = ML2*LcL2*(Ll *(ThOd+ThLld) A 2*sin(Thl.2)+LO*ThOd A 2*... 
sin(ThLl+ThL2)); 

Pt 1 = R0*Th0d A 2*(MR 1 *LcR 1 +MR2*R 1 )*sin(ThR 1 ); 

Pt2 = -MR2*Rl*LcR2*ThR2d*(2*Th0d+2*ThRld+TliR2d)*sin(ThR2). 

Pt3 = MR2*RO*LcR2*ThOd A 2*sin(ThRl+ThR2); 

G(4) = Ptl + Pt2 + Pt3; 

G(5) = MR2*LcR2*(Rl*(Th0d+ThRld) A 2*sin(ThR2)+R0*Th0d A 2*... 
sin(ThR 1 +ThR2)); 
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%%%%%%%%%%%%%% 

%% Constraints Matrix %% 

%%%%%%%%%%%%%% 

% The constraint matrix comes from putting the constraint equations 
% into the PfalTian form: A*qdot + AO = 0. flic first two constraint 
% equations are found by finding the x and y components of the 
% Payload's center of mass by starting at the origin and mov ing 
% up the left arm. The second two constraint equations find the x 
% and y components of the Payload's center of mass by stalling at the 
% origin and moving to the base of the right aim and then 
% up the right arm. Differentiating these equations results 
% in the PfalTian form with AO = 0. 

A = zcros(5,8); 

A(5,l)= 1. 

A(1 ,7) = -1 ; 

A(2,8) = -1; 

A(3,7) = -1; 

A(4,8) = -1; 

A ( 1 ,6) = -LcP*sin(ThP); 

A(2,6) = LcP*cos(ThP), 

A(3,6) = (LP-LcP)*sin(ThP); 

A(4,6) = -(LP-LcP)*cos(ThP); 

A(4,5) = R2*cos(ThO+ThRO+ThRl+ThR2); 

A(4,4) = A(4,5) + R 1 *cos(ThO+ThRO+ThR 1 ); 

A(4,l) = A(4,4) + R0*cos(Th0+ThR0); 

A(3,5) = -R2*sin(Th0+ThR0+ThR 1 +ThR2); 

A(3,4) = A(3,5) - R1 *sin(ThO+ThRO+ThRl); 

A ( 3 , 1 ) = A(3,4) - R0*sin(Th0+ThR0); 

A(2,3) = L2*cos(ThO+ThLO+ThL 1 +ThL2), 

A(2,2) = A(2,3) + L 1 *cos(Th0+ThL0+ThL 1 ); 

A(2,l) = A(2,2) + L0*cos(Th0+ThL0), 

A( 1 ,3) = -L2*sin(Th0+ThL0+ThL 1 +ThL2); 

A( 1 ,2) = A( 1 ,3) - L 1 *sin(ThO+ThLO+ThL 1 ), 

A( 1,1 ) = A( 1 ,2) - L0*sin(Th()+ThL0); 

Adot = zeros(5,8); 

Adot(l ,6) = -ThPd*LcP*cos(ThP); 

Adot(2,6) = -ThPd*LcP*sin(ThP); 

Adot(3,6) = ThPd*(LP-LcP)*cos(ThP); 

Adot(4,6) = ThPd*(LP-LcP)*sin(ThP); 

Adot(4,5) = -(ThOd+ThRld+ThR2d)*R2*sin(ThO+IhRO+ThRl+ThR2); 
Adot(4,4) = Adot(4,5) - (ThOd+ThRld)*Rl*sin(ThO+ThRO+ThRl). 
Adot(4,l ) = Adot(4,4) - ThOd*R()*sin(ThO+ThRO): 

Adot(3,5) = -(ThOd+ThRld+ThR2d)*R2*cos(ThO+ThRO+ThRI+ThR2); 
Adot(3,4) = Adot(3,5) - (ThOd+ThRld)*Rl*cos(ThO+ThRO+ThRl); 
Adot(3,l) = Adot(3,4) - Th0d*R0*cos(Th0+ThR0); 

Adot(2,3) = -(ThOd+ThL 1 d+ThL2d)*L2*sin(Th()+Thl ,()+ThT 1 +Thl.2); 
Adot(2,2) = Adot(2,3) - (ThOd+ThL ld)*Ll*sin(Th()+ThLO+ThLl): 
Adot(2,l) = Adot(2,2) - Th0d*L0*sin(Th0+'l'hL0); 

Adot( 1,3) = -(ThOd+ThL 1 d+ThL2d)*L2*cos(ThO+Thl ,0+ThLl +ThL2); 
Adot(l,2) = Adot(l,3) - (ThOd+ThL ld)*Ll*eos(Th0+ThL0+ThLl); 
Adot(l,l) = Adot(l,2) - ThOd *L0*cos(Th0+ThL0); 

%%%%%% 

%% B %% 

%%%%%% 

B = zeros(8,6); 

B( 1,3) = -1; 

13(1,6) = -1; 

B(2,l) = 1; 
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13(2,3) = -1 
13(3,2) = 1 
13(3,3) = -l 
13(4,4) = 1 
13(4,6) = -1 
13(5,5) = 1 
13(5,6) = -l 
13(6,3)= 1 
B (6,6)= 1 



K. Rea 



% Filename is ’Ref2.ni' 

% Reference Maneuver using cost function 
% This routine assumes that the spacecraft ccnterbody is held fixed 

function [Torques, QRef,QdotRef,Aqdot,J,Cl,C2,C3] = ... 

Ref2(Ls,Ms,CMs,ls,F3oundC,T,Wu,We,Coef,ConstMat) 

% OUTPUTS: 

% Torques = 7x1 eolumn vector of torques that should be applied at 
% time T if the motion is to follow the reference trajectory 

% exaetly. The vector is arranged as [UO; ULS; ULE; (JEW; URS, URE; URW] 

% which are the centerbody torque followed by the torques at the 

% shoulder, elbow, and wrist of the left arm and then the right arm 

% respectively. 

% QRef = 8x1 column veetor of reference generalized coordinates 
% QdotRcf =8x1 column vector of reference generalized velocities 
% Aqdot = 4x1 or 5x1 column vector (depends on status of AMatFlag) whieh 
% check to see if the constraint equation A*Qdot = 0 is satisfied 
% J = scalar value of the reaction wheel torque absolute value. This 
% number will be integrated to find the value for the cost function. 

% Lyapunov Controller matrices (reference trajectory values) 

% C 1 = 8x7 matrix 

% C2 = 8x4 or 8x5 (depends on status of AMatFlag) matrix 
% C3 = 8x1 matrix 

% 

% INPUTS: 

% Ls = 7x 1 vector of lengths (m) 

% 1st element = distance from origin to left arm mount 
% 2nd & 3rd elements wrt left arm (from shoulder toward wrist) 

% 4th element = payload length 

% 5th & 6th elements wrt light arm (from wrist toward shoulder) 

% 7th element = distance from right arm mount to origin 

% [LO; L 1 ; L2; LP; R2; R 1 ; RO] 

% Ms = 6x1 column vector containing the masses (kg) 

% 1 st element = mass of spacecraft centerbody 

% 2nd & 3rd elements = mass of left arm (upper arm then lower aim) 

% 4th & 5th elements = mass of right arm (upper arm then lower arm) 

% 6th element = payload mass 
% [MO; ML 1; ML 2; MR1; MR2; MP] 

% CMs = 6x1 column veetor containing center of mass locations 
% [LcO; LcL 1 ; LeL2; LcR 1 ; LcR2; LcP j 

% ls = 6x1 column vector containing the moments of inertias about the 
% respective body's center of mass (kg m A 2) 

% 1st element = inertia of spacecraft centerbodv 
% 2nd & 3rd elements = inertia of left arm (upper aim then lower arm) 

% 4th & 5th elements = inertia of right arm (upper aim then lower arm) 

% 6th element = payload inertia 



150 



% (10; 1 L 1 ; IL2, IR1; IR2; IP] 

% BoundC = bouncin' conditions for the problem. The first column 
% contains the initial x and y component of points Q & P 

% respectively, the x component of the right arm base, the 

% problem stall time, and the simulation stop time. The seeond 
% column contains the x and y component of points Q & P 

% respectively, the x component of the right aim base, the 

% stop time for the ideal reference maneuver, and a Hag to 
% activate or deactivate the controller. The origin for the 
% x and y components is the base of the left arm 
% T = time 

% Wu = 6x6 or 7x7 control torque cost weighting matrix 
% Wc = 8x8 constraint cost weighting matrix 

% Coef = (n-2)xl column vector of reference polynomial coefficients 
% beginning with order n coefficient 

% ConstMat = 3x(n-2) matrix of coefficients for reference displacement 
% (row 1), velocity (row 2), and acceleration (row 3) 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Lengths (m) 

L0 = Ls(l); 

LI = Ls(2); 

L2 = Ls(3); 

LP = Ls(4); 

R2 = Ls(5); 

R I = Ls(6); 

R0 = Ls(7); 

% Member masses (kg) 

MO = Ms(l); 

ML1 = Ms(2); 

ML2 = Ms(3); 

MR1 = Ms(4); 

MR2 = Ms(5); 

MP = Ms(6); 

% Center of mass distances (m) 

LcO = CMs(l); 

LcLl =CMs(2); 

LeL2 = CMs(3); 

LeRI = CMs(4); 

LcR2 = CMs(5); 

LcP = CMs(6); %measured from left end 

% MOI about center of mass 
10 = Is( 1 ); 

1L1 = ls(2); 

1L2 = Is(3), 

IR1 = Is(4); 

1R2 = Is(5); 

IP = Is(6); 

% Initial and Final locations of third link 
% Point Q is at Node 3 (joint between Links 2 & 3) 

% Point P is at Node 4 (joint between Links 3 & 4) 

QxO = BoundC( 1,1); QyO = BoundC( 1 ,2); 

PxO = BoundC (2, 1); P>'0 = BoundC(2,2); 

Qxf = BoundC(3,I); Qyf = BoundC(3,2), 

Pxf = BoundC(4, 1 ); Pyf = BoundC(4,2); 
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% Arms mount locations wrt spacecraft ccntcrbodv coordinate frame (rad) 
ThLO = BoundC(5, 1); ThRO = BoundC(5,2). 

% Reference Maneuver Start and Stop Times 
TO = BoundC(6,l); Tf =BoundC(6,2); 

% Constraints Matrix Flag 
AMatFlag = BoundC(8,I); 

% Centerbody Reaction Wheel Flag 
Wheel Flag = BoundC(8,2), 

% Centerbody Initial and Final Conditions 
ThOO = BoundC(9, 1 ); 

ThOf = BoundC(9,2); 

% Number of equations in the cost function constraint equations 
EOMFlag = BoundC(10,l); 

% Psuedo-Inverse Flag 
PInvFlag = BoundC(10,2); 



%%%%%%% 0 /o 0 /o 0 /o% 0 /o 0 /o%%% 0 /o% 0 /o%%% 

%% PRELIMINARY CALCULATIONS %% 

%%%%%%%%%%%% 0 /o 0 /o%%% 0 /o%% 0 /o% 

R2D = 180/pi, % Conversion from radians to degrees 

% Total rotation of Payload 

ThPO = atan2(PyO-Qy(),PxO-QxO); % Initial angle of Payload (rad) 

ThPf = atan2(Pyf-Qyf,Pxf-Qxf); % Final angle of Payload (rad) 

DelTliP = ThPf - TliPO; % Total delta angle of Payload (rad) 

% Initial and final locations of Payload center of mass 
XPO = QxO + (PxO - QxO) * (LcP/LP); 

YPO = QyO + (PyO - QyO) * (LcP/LP); 

XPf = Qxf + (Pxf - QxQ * (LcP/LP), 

YPf = Qyf + (Pyf - QyO * (LcP/LP); 

Tau = (T-TO) / (Tf-TO); % Normalize time 

% Function Weighting Factors for how the payload will move 
% These factors will cause the velocity and acceleration of 
% the payload coordinates to be zero at t = 0 and t = tf. 

% They also permit the displacements for the payload coordinates 
% to match their initial and final values. These weighting 
% factors will also apply to the centerbody rotation. 

k = lcngth(Coef); 
for n=l:k 

CTau(k+I-n) = Coef(k+I -n)*Tau A (n+2); 

CTaud(k+I-n) = Cocf(k+I -n)*Tau A (n+I ), 

CTaudd(k+l -n) = Cocf(k+l -n)*Tau A (n); 
end 

% Weighting factors 
W =ConstMat(I,:)*CTau'; 

Wd = ConstMat(2,:)*CTaud'; 

Wdd = ConstMat(3,:)*CTaudd’; 

% Centerbody angle, angular velocity, angular acceleration 
DclThO = ThOf - ThOO; 
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ThO = ThOO + W * Del l h(); % Angle (rad); 

TliOd = Wd * Del ThO / (Tf - TO); % Veloeitv (rad/see); 

ThOdd = Wdd * DclTliO / (Tf - TO) A 2; % Acceleration (rad/sce A 2). 
%ThO =0; 

% TliOd =0; 

% ThOdd = 0; 

% Save for plotting 
QRcf( I ) = ThO, 

QdotRcf( 1 ) = ThOd, 

QddotRef(l) = ThOdd, 

% Payload angle, angular velocity, angular acceleration 
Till’ = ThPO + W * DelThP; % Angle (rad) 

ThPd = Wd * DelThP / (Tf - TO); % Velocity (rad/see) 

ThPdd = Wdd * DelThP / (Tf - T0) A 2; % Acceleration (rad/scc A 2) 
% Save for plotting 
QRef(6) = ThP, 

QdotRef(6) = ThPd; 

QddotRef(6) = ThPdd, 

% Payload center of mass position, velocity, and acceleration 
Xe = XPO + W * (XPf - XPO); 

Xcd = Wd * (XPf - XPO) / (Tf - TO), 

Xedd = Wdd * (XPf - XPO) / (Tf - T0) A 2; 

Yc = YPO + W * (YPf - YPO), 

Yed = Wd * (YPf - YPO) / (Tf - TO); 

Yedd = Wdd * (YPf - YPO) / (Tf - T0) A 2; 

% Save for plotting 
QRef(7) = Xc; 

QdotRef(7) = Xed; 

QddotRef(7) = Xedd; 

QRcf(8) = Yc; 

QdotRef(8) = Yed; 

QddotRef(8) = Ycdd; 

% Payload endpoint coordinates: Qx, Qy, Px, Py 
Qx = Xc - LeP * cos(ThP); 

Qy = Ye - LeP * sin(ThP); 

Px = Xc + (LP - LeP) * cos(ThP); 

Py = Yc + (I,P - LeP) * sin(ThP); 



%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%% Solve for Arm Angles Required by desired path %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%% 

%% LEFT ARM %% 

%%%%%%%%%%% 

% Elbow is left of line from arm base to Q (RQ) 

LSx = L0 * cos(ThO + TliLO); 

LSy = L0 ♦ sin(ThO + ThLO); 

RQ = sqrt((Qx-LSx) A 2+(Qy-LSy) A 2); % Length from ami base to Q 
Betal = atan2(Qy-LSy,Qx-LSx); % Angle from arm base to RQ 

% Law of cosines: cos(A) = (b A 2 + c A 2 - a A 2)/(2be) 

% Apply to find angle between RQ and Link L 1 
Num = L 1 A 2 + RQ A 2 - L2 A 2; 

Den = 2 * LI * RQ; 

Bela2 = aeos(Num/Den); % Angle from RQ to Link 1 

ThLl = (Betal +Beta2)- (ThO + ThLO), %ThetaLl 
% Use law of cosines to find the interior angle at the elbow 
Num = L 1 A 2 + L2 A 2 - RQ A 2; 

Den = 2 * LI * L2, 
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Beta.! = aeos(Num/Den); 
ThL2 = -(pi-Bcla3); 
%Save for plotting 
QRcf(2) = ThL 1 ; 
QRef(3) = ThL2; 



%%%%%%%%%%%% 

%% RIGHT ARM %% 

%%%%%%%%%%%% 

% Elbow is right of line from arm base (shoulder) to P (wrist) (RP) 
RSx = RO * cos(ThO + ThRO), 

RSv = RO * sin(ThO + ThRO); 

RP = sqrt((Px-RSx) A 2+(Py-RSy) A 2); % Length from aim base to P 
Betal = atan2(Py-RSy,Px-RSx); % Angle from arm base to RP 
% Law of cosines; cos(A) = (b A 2 + c A 2 - a A 2)/(2bc) 

% Apply to find angle between RP and Link R1 
Num = R1 A 2 + RP A 2 - R2 A 2; 

Den = 2 * R 1 *RP; 

Beta2 = acos(Num/Den); % Angle from Link R1 to RP 

Bcta4 = Betal - (ThO + ThRO); 

ThRl = -(Beta2 - Bela4); 

Num = R 1 A 2 + R2 A 2 - RP A 2; 

Den = 2 *RI * R2; 

Beta3 = acos(Num/Den); 

ThR2 = pi - Beta3; 

% Save for plotting 
QRcf(4) = ThR I ; 

QRcf(5) = ThR2; 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%% Solve for Arm Angle Rates & Accelerations required by desired path %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%% 

%% LEFT ARM %% 

%%%%%%%%%%% 

% [Qxd; Qyd] = [Hl]*ThOd + [H2]*Thd 
% Qxd & Qyd are x & y components of point Q inertial velocity. 

% Thd = [ThL 1 dot; ThL2dot] 

% H matrices are made from expressing the x&v components of Q in 
% terms of LO, ThO, ThLO, L 1 , ThL 1 . L2, and Till ,2 
% Qx=LO*eos(ThO+ThLO)+L 1 *cos(Th()+ThLO+ThL 1 )+L2*eos(ThO+... 

% ThL0+ThLl+ThL2) 

% Qy=LO*sin(ThO+ThLO)+L 1 *sin(ThO+ThLO+ThL 1 )+L2*sin(ThO+... 

% ' ThLO+ThL 1 +ThL2) 

% The differentiation of these equations lead to 
% [Qxd; Qyd] = [Hl]*ThOd + |H2]*Thd which can be solved for Thd 
Qxd = Xcd + LeP * ThPd * sin(ThP); 

Qx d = Ycd - LcP * ThPd * cos(ThP); 

1 12( 1,2) = -L2*sin(Th0+ThL0+ThL 1 +ThL2); 

112(1,1)= H2(l ,2) - L 1 *sin(ThO+ThLO+ThL 1 ); 

112(2,2) = L2*cos(Th0+ThL0+ThL l+ThL2); 

1 12(2, 1 ) = H2(2,2) + L 1 *cos(ThO+ThLO+Thl. 1 ); 

111(1,1)= H2( 1 , 1 ) - LO*sin(ThO+ThLO); 

111(2,1)= 1 12(2, 1 ) + LO*cos(ThO+ThLO); 

Thd = inv(H2) * ([Qxd; Qyd] - Hl*ThOd); 

% Angle rates 
ThL Id = Thd(l); 

ThL2d = Thd(2); 

% Save for plotting 
QdotRef(2) = ThL Id; 

QdotRef(3) = ThL2d; 
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% Differentiation of |Qxd, Qyd| = [111 ]*Th()d + |1 12|* Hid leads to 
% [Qxdd; Qydd] = (Hldot]*ThOd+|Hl]*Th()dd + |1 l2dot|*Thd+[l 12]*Thdd 
Qxdd = Xedd + LcP*(ThPdd*sin(ThP) + ThPd A 2*cos(ThP)); 

Qydd = Yedd - LcP*(TliPdd*cos(ThP) - ThPd A 2*sin(ThP)), 

1 12dot(l,2) = -L2*(ThOd+ThLld+ThL2d)*cos(ThO+TliLO+ThL l+ThL2), 
112dot(l,l) = H2dot(l,2) - Ll*(ThOd+Thl,ld)*cos(niO+ThLO+Thl.l). 

1 12dot(2,2) = -L2*(Th0d+ThL 1 d+ThL2d)*si»(Th0+ThL0+Thl . 1 +Tlil ,2); 

1 12dot(2,l) = H2dot(2,2) - 1, 1 *(ThOd+ThL ld)*sin(ThO+ThLO+ThL 1 ), 

1 1 1 dot( 1,1)= H2dot( 1,1)- LO*ThOd*cos(ThO+Thl.O); 

1 1 1 dot(2, 1 ) = H2dot(2,l) - LO*ThOd*sin(ThO+ThLO); 

Thdd = inv(I12)*([Qxdd; Qydd]-112dot*Tlid-[lIldot)*Th()d-|lllJ*Th()dd). 
% Angle accelerations 
ThLldd = Thdd(l); 

ThL2dd = Thdd(2); 

QddotRef(2) = ThLldd; 

QddotRef(3) = TliL2dd, 



%%%%%%%%%%%% 

%% RIGHT ARM %% 

%%%%%%%%%%%% 

% The development is similar to the left arm 

% Px=RO*cos(ThO+ThRO)+R 1 *cos(ThO+TliR()+ThRl )+R2*eos(Th()+... 

% ThRO+ThR 1 +ThR2) 

% Py=R()*sin(ThO+ThR())+Rl *sin(ThO+ThRO+ThRl )+R2*sin(Th()+... 

% ' ThRO+ThR l+ThR2) 

% |P\d; Pyd] = [Hl]*ThOd + [H2]*Thd 
Pxd = Xcd - (LP - LcP) * ThPd * stn(ThP); 

Pyd = Ycd + (LP - LcP) * ThPd * cos(ThP), 

112(1,2) = -R2*sin(ThO+ThR()+ThRl+ThR2). 

112(1,1) = 112(1,2) - R1 *sin(ThO+ThRO+ThR 1 ); 

112(2,2) = R2*cos(Th()+ ThRO+ThR 1 +ThR2); 

1 12(2, 1 ) = 112(2,2) + R 1 *cos(ThO+Th RO+Th R 1 ), 

111(1,1) = 112(1,1) - RO*sin(ThO+ThRO). 

H 1 (2, 1 ) = H2(2, 1 ) + RO*cos(ThO+ThRO); 

Thd = inv(H2) * ([Pxd, Pyd] - IIDThOd); 

% Angle rates 
ThRld = Thd(l); 

ThR2d = Thd(2); 

% Save for plotting 
QdotRef(4) = TliRld; 

QdotRef(5) = TliR2d; 

% [Pxdd; Pydd] = [Hldot]*ThOd+[lll]*ThOdd + [I12dot]*Thd+[I12)*Thdd 
Pxdd = Xedd - (LP - LcP)*(ThPdd*sin(ThP) + ThPd A 2*cos(ThP)); 

Pydd = Yedd + (LP - LcP)*(ThPdd*cos(ThP) - ThPd A 2*sin(ThP)); 

H2dot(l ,2) = -R2*(ThOd+ThRld+ThR2d)*co.s(ThO+TliRO+ThRl+ThR2); 
H2dot(l,l) = H2dot(l,2) - Rl*(ThOd+ThRld)*cos(ThO+ThRO+ThRl); 

1 12dot(2,2) = -R2*(ThOd+ThRld+ThR2d)*sin(ThO+ThRO+ThR I +ThR2); 
H2dot(2,l) = H2dot(2,2) - Rl*(ThOd+ThRld)*sm(ThO+ThRO+ThRl); 
Hldot(l,l) = H2dot(l,l) - RO*ThOd*cos(ThO+ThRO); 

Hldot(2,l) = H2dot(2,l) - RO*ThOd*sin(lhO+ThRO); 

Thdd = inv(H2)*([I > xdd; Pydd]-H2dot*Thd-[II ldot]*ThOd-[Hl]*ThOdd); 
% Angle accelerations 
ThRldd = Thdd(l). 

ThR2dd = Thdd(2); 

QddotRef(4) = ThRldd; 

QddotRef(5) = ThR2dd, 



%%%%%%%%%%%%%%%%%%% 
%% f ind needed control torques, u %% 
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%%%%% 0 /o%%%%% 0 /o 0 /o 0 /o%%%%% 

% EOM: M*qddot + G = B*u + A'*Lam 

% Constraint Eqns: A*qdot = 0 
% Solve EOM for qddol leads to 
% qddot = inv(M)*(B*u + A’*Lam - G) 

% Differentiate Constraint Eqns gives Adot*qdot + A*qddot = 0 
% Substitute qddot derived from EOM into differentiated constraint 
% eqns and solve for Lam 

% Lam = dnv(A*inv(M)Vv , )*(A*inv(M)*(B*u-G)+Adol*qdol) 

% Substitute this expression for Lam into ongianl EOM. Collect terms 
% into the form MTilda*qddol + GTilda = BTilda*u 
% where MTilda = M 

% GTilda = G + A'*inv(A*inv(M) t A')*(Adot*qdot-A , * : inv(M) t G) 

% BTilda =(l-A'*inv(A*inv(M)*A')*A*inv(M))*B 

% The first five resulting equations apply to die spacecraft centerbody 
% and arms. The final three apply to the payload. The matrix form of 
% the last three equations is 
% MPTilda*QPddot + GPTilda = BPTilda*u 

%%%%%%%%% 

%% Matrices %% 

%%%%%%%%% 

AngConst = [ThLO; ThRO], 

%AMalFlag = 1; 
if AMatFlag 

| M,G,A,Adol,B j = MatxFi\(Ls,Ms,CMs,Is,QRef,QdotRef,AngConst); 
else 

[M,G,A,Adol,B] = Matx(Ls,Ms,CMs,Is,QRef,QdotRef,AngConst); 
end 

if WheelFlag 

B7 = [1; 0; 0;0;0;0;0; 0]; 

B = [B7 B]; 
end 

% If the cost function is subject to the constraint that the payload 
% satisfy the reference motion, then three equations of motion are used. 

% To include the centerbody reference motion, use four equations from 
% the equations of motion. 



%%%%%%%%% 

%% MTilda %% 

%%%%%%%%% 

if EOMFlag = 3 % Use only the payload equations 

MITilda = M(6:8,6:8); 
else 

if EOMFlag ~ 5 % Use the spacecraft equations 

MPTilda = M( 1:5,1 ;5); 
else % Use all eight equations 

MPTilda - M; 
end 
end 



%%%%%%%%% 

%% GTilda %% 

%%%%%%%%% 

Qdot = QdotRef; 

GTilda = G + A , *inv(A*inv(M)*A , )*(Adol ,,c Qdol - A*inv(M)*G); 
if EOMFlag — 3 % Use only the pavload equations 

GPTilda = GTilda(6:8,l); 
else 
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if EOMFlag == 5 % Use the spacecraft equations 

GPTilda = GTilda( 1:5,1); 
else % Use all eight equations 

GPTilda = G Tilda; 
end 
end 



%%%%%%%%% 

%% BTilda %% 

%%%%%%%%% 

BTilda = (cye(8) - A'*inv(A*inv(M)*A’)*A*inv(M))*B, 
if EOMFlag == 3 % Use only tlic payload equations 

BPTilda = BTilda(6;8,:); 
else 

if EOMFlag == 5 % Use the spacecraft equations 

BPTilda = BTilda( 1:5,:); 
else % Use all eight equations 

BPTilda = BTilda, 
end 
end 



%%%%%%%%% 

%% G1 &G2 %% 

%%%%%%%%% 

% Use previous expression for Lam and regroup terms into the following 

% form A'*Lam = Kl +K2*u 

K1 = A'*inv(A*inv(M)*A , )*(A*inv(M)*G-Adot*Qdot); 

K2 = -A'*inv(A*inv(M)*A')*A*inv(M)*B; 



%%%%%%%%% 

%% Torques %% 

%%%%%%%%% 

% Torques are calculated to minimize the following cost function: 

% J = 0.5*[u'*Wu*u + LanT*A*Wc*A'*Lam + Tr*Wr*Tr] 

% Subject to the constraint: MP*QPddot + GPTilda - BPTilda*u = 0 
% Combine the constraint into the cost function by multiplying the 
% constraint eqn by another Lagrange multiplier. Gam, and adding that 
% to the cost function. Take the gradient with respect to u results in 
% C Wu+K2'* Wc*K2)*u + K2'*Wc*K 1 - BPTilda^Gam = 0 
% Solve for u 

% u = inv(Wu+K2'*Wc*K2)*(I3PTilda , *Gam - K2’* Wc*K 1 ) 

% Substitute this into the constraint eqn. Solve the result for Gam 
% Gam = inv(BPTilda*inv(Wu + K2 , *Wc*K2)*BPTilda , )^MP*QPddot+ 
% G PTi 1 d a+BPTi Id a *i n v(Wu + K2 , *Wc*K2)*K2 , *Wc*K 1) 

% Substitute this expression into the torque equation, u. 

Qddot = QddotRef ; 

if EOMFlag = 3 % Use only the payload equations 

QPddot = Qddot(6:8,l); 
else 

if EOMFlag == 5 % Use the spacecraft equations 

QPddot = Qddot( 1:5,1); 
else % Use all eight equations 

QPddot = Qddot; 
end 
end 



%%%%%%%%%%%%%%%% 
%% PSUEDO-IN VERSES %% 
%%%%%%%%%%%%%%%% 
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% To avoid the problems with poorly conditioned matrices. I've used the 
% psuedo-inverse rather than the traditional inverse in the next two 
% equations 
if PInvFlag 

Parti = pinv(Wu + K2’*We*K2); 

Ci am = pinv(BPTilda*Part I *BPTilda’) * (MPTilda*QPddol + GPTilda +... 
BPTi Ida* Part 1 *(K2'* We*K I )); 

else 

Parti = inv(Wu + K2'*We*K2); 

Gam = inv(BPTilda*Part 1 *BPTilda’) * (MPTiIda*QPddot + GPTilda +... 
BPTi Ida* Part 1 *(K2'* Wc*K 1 )); 

end 

% Reference Torques 

Torques = Parti *(BPTilda’*Gam - K2’*Wc*Kl); 

% Cost Function, J 
J = abs(Torques( 1 )); 

%Controller Info 

Pll = A'*inv(A*inv(M)*A'); 

Cl = inv(M)*(eye(M) - Pll*A*inv(M))*B; 

C2 = -inv(M)*Ptl*Adot; 

C3 = inv(M)*(Pll *A*inv(M) - eye(M))*G; 



%%%%%%%%%%%% 

%% DEBUG INFO %% 

%%%%%%%%%%%% 

%% Are constraint equations, A*qdot=0, satisfied? 
Aqdot = A*QdotRef; 



L. RefMin2 

% Filename is ’RefMin2.m’ 

% Reference Maneuver using cost function 

% This routine is used by H MainOpl.m M to find the optimal combination 
% of reference trajeetoiy polynomial coefficients. 

% Version 2 uses the rate of change of angular momentum to find 
% the wheel torque. 

function [Joptl ,Jopt2] = RefMin2(T,Ls,Ms,CMs,Is,BoundC,Wu,Wc,Coef,ConstMal) 
% OUTPUTS: 

% Jopl = absolute value of the reaction wheel torque. This is the cost 
% function value for purposes of optimizing the reference 
% trajectory polynomial coefficients. Joptl will be integrated by 
% odemin.m while Jopl2 is the same value but won't be integrated. 

% 

% INPUTS: 

% T = time (sec) 

% Ls = 7x1 vector of lengths (m) 

% 1 si element = distance from origin to left aim mount 

% 2nd & 3rd elements wrt left arm (from shoulder toward wrist) 

% 4th element = payload length 

% 5lli & 6th elements wrt right arm (from wrist toward shoulder) 

% 7th element = distance from right arm mount to origin 

% [LO; LI; L2; LP; R2; Rl; RO] 

% Ms = 6x1 column vector containing the masses (kg) 

% 1 st element = mass of spacecraft eenterbody 



158 



% 2nd & 3rd elements = mass of left arm (upper arm then lower arm) 

% 4th & 5th elements = mass of right ami (upper ami then lower arm) 

% 6th element = payload mass 
% | MO, ML1; ML2; MR1; MR2; MP] 

% CMs = 6x1 column vector containing center of mass locations 
% [LcO; LcL 1 ; LcL2, LcRl; LcR2; LeP] 

% Is = 6x1 column vector containing the moments of inertias about the 
% respective body's center of mass (kg m A 2) 

% 1 st element = inertia of spacecraft ccntcrbody 

% 2nd & 3rd elements = inertia of left arm (upper arm then lower ami) 
% 4th & 5th elements = inertia of right ami (upper arm then lower ami) 
% 6th element = payload inertia 
% (10; 1L1; IL2; 1R1; 1R2; IP] 

% BoundC = boundry conditions for the problem. The first column 
% contains the initial x and y component of points Q & P 
% respectively, the x component of the right ami base, the 
% problem start time, and the simulation stop time. The second 
% column contains the x and y component of points Q & P 
% respectively, the x component of the right arm base, the 
% stop time for the ideal reference maneuver, and a Hag to 
% activate or deactivate the controller The origin for the 
% x and y components is the base of the left aim. 

% Wu = 6x6 control torque cost weighting matrix 
% Wc = 8x8 constraint cost weighting matrix 

% Cocf = (n-2)xl vector of polynomial reference trajectory coefficients 
% in descending order where n is the highest order coefficient 

% ConstMat = 3x(n-2) matrix of coefficients for reference trajectory 
% displacement (row 1), velocity (row 2) and acceleration (row 3) 



%%%%%%%%% 0 /o 0 /o 0 /o 0 /o 0 /o 0 /o% 0 /o 0 /o 0 /o 0 /o%%%%%%%% 0 /o%% 

%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 

%%%%%%%%%%%%% 0 /o 0 /o 0 /o 0 /o% 0 /o%%%%%%%%%%%% 

% Lengths (m) 

LO = Ls(l): 

LI = Ls(2); 

L2 = Ls(3); 

LP = Ls(4), 

R2 = Ls(5); 

R1 = Ls(6), 

RO = Ls(7); 

% Member masses (kg) 

MO = Ms(l); 

ML1 = Ms(2); 

ML2 = Ms(3); 

MR1 =Ms(4); 

MR2 = Ms(5), 

MP = Ms(6); 

% Center of mass distances (m) 

LcO = CMs(l); 

LeLl = CMs(2); 

LcL2 = CMs(3); 

LcRl = CMs(4), 

LcR2 = CMs(5); 

LcP = CMs(6); %measured from left end 

% MOl about center of mass 
10 =ls(l); 

IL1 = Is(2); 

1L2 = Is(3); 
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IRI = Is(4); 
IR2 = Is(5); 
IP = Is(6); 



% Initial and final locations of third link 
% Point Q is at Node 3 (joint between Links 2 & 3) 
% Point P is at Node 4 (joint between Links 3 & 4) 
QxO = BoundC( 1 , 1 ); QyO = BoundC( 1 ,2), 

Px() = BoundC(2,I); PyO = BoundC(2,2); 

Qxf = BoundC(3,l); Qyf = BoundC(3,2); 

Pxf = BoundC(4,I); Pyf = BoundC(4,2); 



% Anns mount locations wrt spacecraft centerbody coordinate frame (rad) 
ThLO = BoundC(5,l); ThRO = BoundC(5,2); 



% Reference Maneuver Start and Stop Times 
TO = BoundC(6,I); Tf = BoundC(6,2); 



% Constraints Matrix Flag 
AMatFIag = BoundC(8,l); 

% Centerbody Reaction Wheel Flag 
Wheel Flag = BoundC(8,2); 

% Centerbody Initial and Final Conditions 
ThOO = BoundC(9, 1 ); 

ThOf = BoundC(9,2); 

% Number of equations in the cost function constraint equations 
LOMFlag = BoundC(10,l); 

% Psuedo-Inverse Flag 
PlnvFlag = BoundC(10,2), 



%%%%%%%%%%%%%% 0 /o% 0 /o%%%%% 

%% PRELIMINARY CALCULATIONS %% 
%%%%%%%%%%%%%%%%%%%%%% 

R2D = 1 80/pi; % Conversion from radians to degrees 

% Total rotation of Payload 

ThPO = atan2(PyO-QyO,PxO-QxO); % Initial angle of Payload (rad) 

ThPf = atan2(Pyf-Qyf,Pxf-Qxf); % Final angle of Payload (rad) 

DelThP = ThPf - ThPO; % Total delta angle of Payload (rad) 

% Initial and final locations of Payload center of mass 
XPO = QxO -f- (PxO - QxO) * (LcP/LP); 

YPO = QyO + (PyO - QyO) * (LcP/LP); 

XPf = Qxf + (Pxf - Qxf) * (LcP/LP); 

YPf = Qyf + (Pyf - QyO * (LcP/LP); 

Tau = (T-TO) / (Tf-TO); % Normalize time 

% Function Weighting Factors for how the payload will move 
% These factors will cause the angular velocity and angular 
% acceleration of the payload to be zero at t = 0 and t - tf 
% They also permit the payload angle to match its initial 
% and final values. These weighting factors will also apply 
% to the translational motion of the payload center of mass. 

k = length(Coef); 
for n=I:k 
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CTau(k+ 1 -n) = Coef(k+l -n)*Tau A (n+2); 

CTaud(k+l-n) = Cocf(k+l-n)*Tau A (n+l); 

CTaudd(k+l-n) = Coef(k+l-n)*Tau A (n); 
end 

W = ConstMat(l,:)*CTau'; 

Wd = ConstMat(2,:)*CTaud'; 

Wdd = ConstMat(3,:)*CTaudd'; 

% Centerbody angle, angular velocity, angular acceleration 
DclThO = ThOf - ThOO; 

ThO = ThOO + W * DelThO; % Angle (rad), 

ThOd = Wd * DclThO / (Tf - TO); % Velocity (rad/sec); 

ThOdd = Wdd * DelThO / (Tf - T0) A 2; % Acceleration (rad/sec A 2); 
% Save for plotting 
QRef( 1 ) = ThO; 

QdotRcfd) = ThOd; 

QddotRcf(l) = ThOdd; 

% Payload angle, angular velocity, angular acceleration 
ThP = ThPO + W * DelThP; % Angle (rad) 

'I hl’d = Wd * DelThP / (Tf - TO); % Velocity (rad/see) 

ThPdd = Wdd * DelThP / (Tf - T0) A 2; % Acceleration (rad/see A 2) 
% Save for plotting 
QRef(6) = ThP; 

QdotRef(6) = ThPd; 

QddotRef(6) = ThPdd; 

% Payload center of mass position, velocity, and acceleration 
Xe ='XP0 + W * (XPf - XPO); 

Xed = Wd * (XPf - XPO) / (Tf - TO); 

Xedd = Wdd ♦ (XPf - XPO) / (Tf - T0) A 2; 

Ye = YPO + W * (YPf - YPO); 

Yed = Wd * (YPf - YPO) / (Tf - TO); 

Yedd = Wdd * (YPf - YPO) / (Tf - T0) A 2; 

% Save for plotting 
QRef(7) = Xe; 

QdotRcf(7) = Xed; 

QddotRef(7) = Xedd; 

QRef(8) = Ye; 

QdotRef(8) = Ycd. 

QddotRef(8) = Yedd; 

% Payload endpoint coordinates; Qx, Qy, Px, Py 
Qx = Xe - LcP * eos(ThP); 

Qy = Ye - LeP * sin(ThP); 

Px = Xe + (LP - LeP) * cos(ThP); 

Py = Ye + (LP - LeP) * sin(ThP); 



%%%%%%%%%%%%%%%%%%%%%%%%%% 

%% Solve for Ann Angles Required by desired path %% 
%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%% 

%% LEFT ARM %% 

%%%%%%%%%%% 

% Elbow is left of line from ann base to Q (RQ) 

LSx = LO * eos(ThO + TliLO); 

LSy = LO * sin(ThO + ThLO); 

RQ = sqrt((Qx-LSx) A 2+(Qy-LSy) A 2); % Length from arm base to Q 
Beta! = atan2(Qy-LSy,Qx-LSx); % Angle from ann base to RQ 

% Law of cosines; cos(A) = (b A 2 + e A 2 - a A 2)/(2bc) 

% Apply to find angle between RQ and Link L 1 
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Num = L 1 A 2 + RQ A 2 - L2 A 2; 

Den = 2 * LI + RQ, 

Beta2 = acos(Num/Den); % Angle from RQ to Link I 

ThL 1 = (Beta I + Bcta2) - (Th() + ThLO); % Theta LI 
% Use law of cosines to find the interior angle at the elbow 
Num = L 1 A 2 + L2 A 2 - RQ A 2; 

Den =2 * LI * L2; 

Bcta3 = acos(Num/Den); 

ThL2 = -(pi-Beta3); 

%Savc for plotting 
QRef(2) = ThL I; 

QRcf(3) = ThL 2; 



%%%%%%%%%%%% 

%% RIGHT ARM %% 

%%%%%%%%%%%% 

% I dhow is right of line from arm base (shoulder) to P (wrist) (RP) 

RSx = RO * cos(ThO + ThRO); 

RSy = RO * sin(ThO + ThRO); 

RP = sqrt((Px-RSx) A 2+(Py-RSv) A 2); % Length from arm base to P 
Beta I = atan2(Py-RSy,Px-RSx); % Angle from ann base to RP 
% Law of cosines: cos(A) = (b A 2 + c A 2 - a A 2)/(2bc) 

% Apply to find angle between RP and Link R I 
Num = R I A 2 + RP A 2 - R2 A 2; 

Den = 2 * R 1 * RP; 

Beta2 = acos(Num/Den); % Angle from Link R1 to RP 

Beta4 = Betal - (ThO + ThRO); 

ThRI = -(Beta2 - Beta4); 

Num = R1 A 2 + R2 A 2 - RP A 2; 

Den = 2 * R 1 * R2, 

Beta3 = acos(Num/Den); 

ThR2 = pi - Beta3; 

% Save for plotting 
QRef(4) = ThRI ; 

QRef(5) = ThR2; 

%%%%%%%%%%%%%%%%%%%%%%%%% 

%% Solve for Arm Angle Rates & Accelerations %% 

%% required by desired path %% 

%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%% 

%% LEFT ARM %% 

%%%%%%%%%%% 

% |Qxd. Qyd] = [HI]*ThOd + [112] "Thd 
% Qxd & Qyd arc x & y components of point Q inertial velocity. 

% Thd = [ThL I dot ; ThL2dot] 

% 1 1 matrices are made from expressing the x & v components of Q in 
% terms of LO, ThO, ThLO, Ll.ThLl, L2, and ThL2. 

% Qx=LO*cos('fhO+ThLO)+L 1 *cos(ThO+Thl ,0+ThL 1 )+L2*cos(ThO+. .. 
ThLO+ThL 1 +ThL2) 

% Qy=LO*sin(ThO+ThLO)+L I *sin(ThO+ThLO+ThL I )+L2*sin(ThO+... 
ThLO+ThL I +ThL2) 

% The differentiation of these equations lead to 
% [Qxd; Qyd] = [Hl]*ThOd + [H2]*Thd which can be solved for Thd 
Qxd = Xcd + LcP * ThPd * sin(ThP); 

Qvd = Ycd - I,cP * ThPd * cos(ThP); 

1 12(1,2) = -L2*sin(Th0+ThL0+ThL 1 +ThI,2); 

112(1,1) = H2(l,2) - LI *sin(ThO+ThLO+ThL I ); 

112(2.2) = L2*cos(ThO+ThLO+ThL 1 +ThL2); 

1 12(2, 1 ) = H2(2,2) + L 1 *cos(ThO+ThLO+ThL 1 ); 

H I ( 1 , 1 ) = H2( 1,1) - LO*sin(ThO+ThLO); 
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111(2,1) = 112(2,1) + LO*eos(ThO+ThLO); 

Thd = inv(l 12) * (|Qxd, Qyd] - 1 1 1 *Th()d). 

% Angle rates 
ThL Id = Thd(I); 

ThL 2d = Thd(2); 

% Save for plotting 
QdotRcf(2) = ThL 1 d, 

QdotRcf(3) = ThL2d, 

% Differentiation of [Qxd, Qvd] = [Hl]*ThOd + |1 12]*Thd leads to 
% |Qxdd; Qydd| = [Hldot]*ThOd+[Hl]*ThOdd + |II2dot|*Thd+[ll2|*Thdd 
Qxdd = Xedd + LcP*(ThPdd*sin(ThP) + ThPd A 2*cos(ThP)); 

Qydd = Yedd - LcP*(ThPdd*cos(ThP) - ThPd A 2*sin(ThP)); 

I I2dot( 1,2) = -L2*(Th0d+ThL 1 d+ThL2d)*cos(Th(>+ThL0+ThL 1 +ThL2), 

1 12dot( 1 ,1 ) = H2dot(l,2) - lJ*(Th()d+Thl,ld)*eos(Th()+Tld,()+Tlil,l), 

1 12dot(2,2) = -L2*(Th()d+ThLld+ThL2d)*sin(Th()+ThL0+ThL 1 +Thl .2). 
H2dot(2,l) = H2dot(2,2) - Ll*(ThOd+ThLld)*sin(Th()+Thh()+Thl.l). 
Hldot(l,l) = H2dot(l ,1) - L0*Th0d*cos(Th()+Thl,0), 

1 1 1 dol(2, 1 ) = H2dot(2,l) - LO*Th()d*sin(ThO+ThLO). 

Thdd = inv(H2)*([Qxdd; Qydd]4l2dot*Thd-[l lldot]*Th()d-|Ill |*ThOdd), 
% Angle accelerations 
ThLldd = Thdd(l); 

ThL2dd = Thdd(2); 

QddotRef(2) = ThLldd, 

QddotRef(3) = ThL2dd. 



%%%%%%%%%%%% 

%% RIGHT ARM %% 

%%%%%%%%%%%% 

% The development is similar to the left arm 

% Px=RO*cos(ThO+ThRO)+R 1 *cos(ThO+ThRO+ThR 1 )+R2*cos(ThO+, . . 
ThRO+ThR 1 +ThR 2 ) 

% Pv=R()*sin(Th()+ThRO)+Rl*sm(Th()+ThRO+ThRl)+R2*sin(ThO+... 

TliI^0+ThRl+ThR2) 

% fPxd; Pyd] = [Hl]*ThOd + |H2]*Thd 
Pxd = Xcd - (LP - LcP) * ThPd * sin(ThP), 

Pvd = Ycd + (LP - LcP) * ThPd * cos(ThP): 

112(1,2) = -R2*sin(ThO+ThRO+ThRI+ThR2); 

H2( 1,1) = H2(l,2) - R1 *sin(ThO+ThR()+ThRl ); 

112(2,2) = R2*cos(Th()+ThR0+ThR 1 +ThR2); 

1 12(2, 1 ) = 1 12(2,2) + R 1 *cos(ThO+ThRO+ThR 1 ), 

111(1,1)= H2(l ,1 ) - RO*sin(ThO+ThRO); 

111(2,1)= H2(2, 1 ) + R()*cos(ThO+ThRO). 

Thd = inv(1 12) * ([Pxd, Py-d] - 1 1 1 *Th()d), 

% Angle rates 
ThRld = Thd(l); 

ThR2d = Thd(2); 

% Save for plotting 
QdotRef(4) = ThRld; 

QdotRef(5) = Thl^2d, 

% [Pxdd, Pydd] = [Hldot]*ThOd+[Hl ]*ThOdd + [1 12dot|*Thd+|l 12|*Thdd 
Pxdd = Xedd - (LP - LcP)*(ThPdd*sin(ThP) + ThPd A 2*cos(ThP)), 

Pydd = Yedd + (LP - LcP)*(ThPdd*cos(ThP) - ThPd A 2*sin(ThP)), 

1 12dot( 1 ,2) = -R2*(ThOd+ThRld+ThR2d)*cos(Th()+ThR()+ThRl+ThR2). 
H2dot( 1,1)= H2dot( 1 ,2) - R 1 *(ThOd+ThR 1 d)*cos(ThO+ThRO+ThR I ); 
H2dot(2,2) = -R2*(Th()d+ThR ld+ThR2d)*sin(Th()+ThR0+ThR 1 +ThR2); 

1 12dot(2, 1 ) = H2dot(2,2) - R 1 *(ThOd+ThR 1 d)*sin(ThQ+ThRO+ThR 1 ); 
Hldot(l.l) = H2dot(l,l) - RO*ThOd*cos(ThO+ThRO); 

Hldot(2,l) = H2dot(2,l) - RO*ThOd*sin( I hO+ThRO); 

Thdd = inv(H2)*([Pxdd; Pydd]-H2dot*Thd-[Hldot]*Th()d-[I Il]*ThOdd); 
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% Angle accelerations 
ThRldd = Thdd(l); 

ThR2dd = Thdd(2); 

QddotRef(4) = ThRldd; 

QddotRef(5) = ThR2dd; 

%%%%%%%%%%%%%%%%%%%% 

%% Find needed eontrol wheel torque %% 

%%%%% 0 /o 0 /o 0 /o%%%%%%% 0 /o 0 /o%%% 

Q = QRef; 

Qdot = QdotRef ; 

Qddot = QddotReF; 

[Hs, Hdots] = AngMo2(Ls,Ms,CMs,Is,Q,Qdot, Qddot), 

% Cost Function, Jopt 

% Wheel torque is the change in total angular momentum 
% Jopt 1 is integrated while Jopt 2 is not 
Joptl = abs(Hdots(7)); 

Jopt2 = Jopt 1 ; 
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